xtdrone
/
XTDrone
1
0
Fork 15
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://gitee.com/robin_shaun/XTDrone

This commit is contained in:
Peter 2020-05-24 16:07:28 +08:00
parent d4ebc428dc 46aa2d6b6b
commit f9d19140ed
314 changed files with 359592 additions and 278642 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -3,5 +3,6 @@ competition
*.bag
*.pyc
*.pbstream
sensing/slam/vio/VINS-Fusion/camera_models/camera_calib_example
.gitee
.vscode

18
README.en.md
View File

@ -26,31 +26,35 @@ Developers can quickly verify algorithms with XTDrone, such as:
<img src="./image/human_tracking.gif" width="640" height="368" />
2. Visual SLAM
2. Stereo SLAM
<img src="./image/vslam.gif" width="640" height="368" />
3. 2D Laser SLAM
3. RGBD-SLAM
<img src="./image/rgbdslam.gif" width="640" height="368" />
4. 2D Laser SLAM
<img src="./image/laser_slam_2d.gif" width="640" height="368" />
4. 3D Laser SLAM
5. 3D Laser SLAM
<img src="./image/laser_slam_3d.gif" width="640" height="368"/>
5. VIO
6. VIO
<img src="./image/vio.gif" width="640" height="368" />
6. Motion Planning
7. Motion Planning
<img src="./image/motion_planning.gif" width="640" height="368" />
7. Formation
8. Formation
<img src="./image/cooperation.gif" width="640" height="368" />
8. Self driving
9. Self driving
<img src="./image/self_driving.gif" width="640" height="368" />

20
README.md
View File

@ -26,31 +26,35 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
<img src="./image/human_tracking.gif" width="640" height="368" />
2. 视觉SLAM
2. 双目SLAM
<img src="./image/vslam.gif" width="640" height="368" />
3. 2D激光SLAM
3. RGBD-SLAM
<img src="./image/rgbdslam.gif" width="640" height="368" />
4. 2D激光SLAM
<img src="./image/laser_slam_2d.gif" width="640" height="368" />
4. 3D激光SLAM
5. 3D激光SLAM
<img src="./image/laser_slam_3d.gif" width="640" height="368"/>
<img src="./image/laser_slam_3d.gif" width="640" height="368"/>
5. 视觉惯性导航
6. 视觉惯性导航
<img src="./image/vio.gif" width="640" height="368" />
6. 运动规划
7. 运动规划
<img src="./image/motion_planning.gif" width="640" height="368" />
7. 多机协同
8. 多机协同
<img src="./image/cooperation.gif" width="640" height="368" />
8. 自动驾驶
9. 自动驾驶
<img src="./image/self_driving.gif" width="640" height="368" />

16
communication/multi_vehicle_communication.sh
View File

@ -1,12 +1,12 @@
#!/bin/bash
iris_num=6
typhoon_h480_num=3
solo_num=6
plane_num=3
rover_num=3
standard_vtol_num=3
tiltrotor_num=3
tailsitter_num=6
iris_num=18
typhoon_h480_num=0
solo_num=0
plane_num=0
rover_num=0
standard_vtol_num=0
tiltrotor_num=0
tailsitter_num=0
vehicle_num=0
while(( $vehicle_num< iris_num))

1
communication/multirotor_communication.py
View File

@ -231,6 +231,7 @@ class Communication:
return False
def hover(self):
self.coordinate_frame = 1
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)

6
control/multirotor_keyboard_control.py
View File

@ -89,9 +89,11 @@ if __name__=="__main__":
multirotor_num = int(sys.argv[2])
control_type = sys.argv[3]
if multirotor_num == 9:
if multirotor_num == 18:
formation_configs = ['waiting', 'cube', 'sphere', 'diamond']
elif multirotor_num == 9:
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
if multirotor_num == 6:
elif multirotor_num == 6:
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
cmd= String()

26
coordination/formation_demo/avoid.py
View File

@ -10,12 +10,12 @@ vehicle_type = sys.argv[1]
vehicle_num = int(sys.argv[2])
pose = [None]*vehicle_num
pose_sub = [None]*vehicle_num
avoid_accel_pub = [None]*vehicle_num
avoid_kp = 0.2
avoid_radius = 1
avoid_vel_pub = [None]*vehicle_num
avoid_kp = 0.5
avoid_radius = 1.5
aid_vec1 = [1, 0, 0]
aid_vec2 = [0, 1, 0]
uavs_avoid_accel = [Vector3()] * vehicle_num
vehicles_avoid_vel = [Vector3()] * vehicle_num
def pose_callback(msg, id):
pose[id] = msg
@ -23,7 +23,7 @@ def pose_callback(msg, id):
rospy.init_node('avoid')
for i in range(vehicle_num):
pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i)
avoid_accel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i+1)+"/avoid_accel", Vector3,queue_size=10)
avoid_vel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_vel", Vector3,queue_size=10)
time.sleep(1)
rate = rospy.Rate(50)
@ -37,14 +37,18 @@ while not rospy.is_shutdown():
cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1))
cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2))
if abs(cos1) < abs(cos2):
avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
else:
avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
uavs_avoid_accel[i] = Vector3(uavs_avoid_accel[i].x+avoid_accel[0],uavs_avoid_accel[i].y+avoid_accel[1],uavs_avoid_accel[i].z+avoid_accel[2])
uavs_avoid_accel[i+j] = Vector3(uavs_avoid_accel[i+j].x-avoid_accel[0],uavs_avoid_accel[i+j].y-avoid_accel[1],uavs_avoid_accel[i+j].z-avoid_accel[2])
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
if dir_vec[2] * avoid_vel[2] > 0:
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x+avoid_vel[0],vehicles_avoid_vel[i].y+avoid_vel[1],vehicles_avoid_vel[i].z+avoid_vel[2])
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x-avoid_vel[0],vehicles_avoid_vel[i+j].y-avoid_vel[1],vehicles_avoid_vel[i+j].z-avoid_vel[2])
else:
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x-avoid_vel[0],vehicles_avoid_vel[i].y-avoid_vel[1],vehicles_avoid_vel[i].z-avoid_vel[2])
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x+avoid_vel[0],vehicles_avoid_vel[i+j].y+avoid_vel[1],vehicles_avoid_vel[i+j].z+avoid_vel[2])
for i in range(vehicle_num):
avoid_accel_pub[i].publish(uavs_avoid_accel[i])
uavs_avoid_accel = [Vector3()] * vehicle_num
avoid_vel_pub[i].publish(vehicles_avoid_vel[i])
vehicles_avoid_vel = [Vector3()] * vehicle_num
rate.sleep()

368
coordination/formation_demo/follower.py
View File

@ -9,6 +9,8 @@ if sys.argv[3] == '6':
from formation_dict import formation_dict_6 as formation_dict
elif sys.argv[3] == '9':
from formation_dict import formation_dict_9 as formation_dict
elif sys.argv[3] == '18':
from formation_dict import formation_dict_18 as formation_dict
import time
import math
import numpy
@ -32,26 +34,20 @@ class Follower:
self.local_pose_queue.put(PoseStamped())
self.local_velocity = TwistStamped()
self.cmd_vel_enu = Twist()
self.cmd_accel_enu = Vector3()
self.avoid_accel = Vector3()
self.avoid_vel = Vector3()
self.following_switch = False
self.arrive_print = True
self.following_ids = []
self.formation_config = 'waiting'
self.following_count = 0
self.Kp = 1000 #100
#self.kr = (4/int((self.uav_num-1)/2))**0.5
self.kr = 1
self.velxy_max = 2
self.velz_max = 2
self.Kp = 1
self.velxy_max = 0.8
self.velz_max = 0.8
self.following_local_pose = [PoseStamped() for i in range(self.uav_num)]
self.following_local_pose_sub = [None]*self.uav_num
self.following_local_velocity = [TwistStamped() for i in range(self.uav_num)]
self.following_local_velocity_sub = [None]*self.uav_num
self.arrive_count = 0
self.local_pose_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.local_velocity_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped, self.local_velocity_callback)
self.avoid_accel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback)
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
self.info_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/info', String, queue_size=10)
@ -65,40 +61,21 @@ class Follower:
pose_comparison = self.local_pose_queue.get()
self.local_pose_queue.put(self.local_pose)
comparison = (self.local_pose.pose.position.x - pose_comparison.pose.position.x)**2+(self.local_pose.pose.position.y - pose_comparison.pose.position.y)**2+(self.local_pose.pose.position.z - pose_comparison.pose.position.z)**2
#if self.id == 6:
#print('comparison1:',comparison)
#print('comparison2:',float(self.velxy_max**2+self.velxy_max**2+self.velz_max**2)/1e3)
#print(self.arrive_count)
if comparison < float(self.velxy_max**2+self.velxy_max**2+self.velz_max**2)/1e5:
#if self.id == 2:
#print('here')
self.arrive_count += 1
else:
self.arrive_count = 0
def local_velocity_callback(self, msg):
self.local_velocity = msg
def following_local_pose_callback(self, msg, id):
self.following_local_pose[id] = msg
def following_local_velocity_callback(self, msg, id):
self.following_local_velocity[id] = msg
def cmd_vel_callback(self, msg):
self.cmd_vel_enu = msg
def formation_switch_callback(self, msg):
if not self.formation_config == msg.data:
self.following_switch = True
self.formation_config = msg.data
self.formation_config = msg.data
def avoid_accel_callback(self, msg):
self.avoid_accel = msg
def avoid_vel_callback(self, msg):
self.avoid_vel = msg
def loop(self):
rospy.init_node('follower'+str(self.id-1))
@ -120,10 +97,9 @@ class Follower:
else:
if self.first_formation:
self.first_formation=False
self.L_matrix = self.get_L_central_matrix()
self.orig_formation=formation_dict[self.formation_config]
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
else:
#self.new_formation=self.get_new_formation(self.orig_formation,formation_dict[self.formation_config])
self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config])
self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left
self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set
@ -132,64 +108,52 @@ class Follower:
self.visit_left = numpy.array([0]*(self.uav_num-1))
self.visit_right = numpy.array([0]*(self.uav_num-1))
self.slack_right = numpy.array([100]*(self.uav_num-1))
self.L_matrix = self.get_L_central_matrix()
self.change_id = self.KM()
self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config])
self.L_matrix = self.get_L_matrix(self.new_formation)
self.orig_formation=self.new_formation
#self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
if self.id==2:
print(self.L_matrix)
#self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1)
#if self.id == 2:
#print(self.following_ids)
self.following_count = 0
for i in range(self.uav_num):
if not self.following_local_pose_sub[i] == None:
self.following_local_pose_sub[i].unregister()
if not self.following_local_velocity_sub[i] == None:
self.following_local_velocity_sub[i].unregister()
for following_id in self.following_ids:
#print('here')
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0])
self.following_count += 1
self.cmd_accel_enu = Vector3(0, 0, 0)
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_accel)
self.cmd_vel_enu.linear = Vector3(0, 0, 0)
for following_id in self.following_ids:
#if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None:
#print(following_id)
self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict[self.formation_config][0, self.id-2]
self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict[self.formation_config][1, self.id-2]
self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict[self.formation_config][2, self.id-2]
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[self.formation_config][0, self.id-2]
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict[self.formation_config][1, self.id-2]
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict[self.formation_config][2, self.id-2]
if not following_id[0] == 0:
self.cmd_accel_enu.x -= formation_dict[self.formation_config][0, following_id[0]-1]
self.cmd_accel_enu.y -= formation_dict[self.formation_config][1, following_id[0]-1]
self.cmd_accel_enu.z -= formation_dict[self.formation_config][2, following_id[0]-1]
if self.arrive_count <= 2000:
self.cmd_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.Kp * (self.avoid_accel.x + self.cmd_accel_enu.x / self.f)
self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y + self.Kp * (self.avoid_accel.y + self.cmd_accel_enu.y / self.f)
self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z + self.Kp * (self.avoid_accel.z + self.cmd_accel_enu.z / self.f)
if self.cmd_vel_enu.linear.x > self.velxy_max:
self.cmd_vel_enu.linear.x = self.velxy_max
elif self.cmd_vel_enu.linear.x < - self.velxy_max:
self.cmd_vel_enu.linear.x = - self.velxy_max
if self.cmd_vel_enu.linear.y > self.velxy_max:
self.cmd_vel_enu.linear.y = self.velxy_max
elif self.cmd_vel_enu.linear.y < - self.velxy_max:
self.cmd_vel_enu.linear.y = - self.velxy_max
if self.cmd_vel_enu.linear.z > self.velz_max:
self.cmd_vel_enu.linear.z = self.velz_max
elif self.cmd_vel_enu.linear.z < - self.velz_max:
self.cmd_vel_enu.linear.z = - self.velz_max
if not self.formation_config == 'waiting':
self.vel_enu_pub.publish(self.cmd_vel_enu)
self.cmd_vel_enu.linear.x -= formation_dict[self.formation_config][0, following_id[0]-1]
self.cmd_vel_enu.linear.y -= formation_dict[self.formation_config][1, following_id[0]-1]
self.cmd_vel_enu.linear.z -= formation_dict[self.formation_config][2, following_id[0]-1]
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z
if self.cmd_vel_enu.linear.x > self.velxy_max:
self.cmd_vel_enu.linear.x = self.velxy_max
elif self.cmd_vel_enu.linear.x < - self.velxy_max:
self.cmd_vel_enu.linear.x = - self.velxy_max
if self.cmd_vel_enu.linear.y > self.velxy_max:
self.cmd_vel_enu.linear.y = self.velxy_max
elif self.cmd_vel_enu.linear.y < - self.velxy_max:
self.cmd_vel_enu.linear.y = - self.velxy_max
if self.cmd_vel_enu.linear.z > self.velz_max:
self.cmd_vel_enu.linear.z = self.velz_max
elif self.cmd_vel_enu.linear.z < - self.velz_max:
self.cmd_vel_enu.linear.z = - self.velz_max
if not self.formation_config == 'waiting':
self.vel_enu_pub.publish(self.cmd_vel_enu)
if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2:
self.arrive_count += 1
else:
self.cmd_pub.publish(self.hover)
self.arrive_count = 0
rate.sleep()
def build_graph(self,orig_formation,change_formation):
@ -214,13 +178,11 @@ class Follower:
return True
else:
self.slack_right[j]=min(gap,self.slack_right[j])
#print('1',slack_right)
return False
def KM(self):
for i in range(self.uav_num-1):
#print(i)
self.slack_right = numpy.array([100]*(self.uav_num-1))
while True:
self.visit_left = numpy.array([0]*(self.uav_num-1))
@ -228,7 +190,6 @@ class Follower:
if self.find_path(i):
break
d = numpy.inf
#print ('2',slack_right)
for j, slack in enumerate(self.slack_right):
if not self.visit_right[j] :
d = min(d,slack)
@ -240,11 +201,6 @@ class Follower:
self.label_right[k] += d
else:
self.slack_right[k] -=d
'''
for j in range(uav_num-1):
if match_right[j] >=0 and match_right[j] < uav_num:
res += adj_matrix[match_right[j]][j]
'''
return self.match_right
def get_new_formation(self,change_id,change_formation):
@ -252,39 +208,6 @@ class Follower:
new_formation=numpy.zeros((3,self.uav_num-1))
position=numpy.zeros((3,self.uav_num-1))
'''
num_array=range(0,self.uav_num-1)
num_array_possi=list(permutations(num_array, self.uav_num-1))
possi_num=len(num_array_possi)
distance=[0 for i in range(self.uav_num-1)]
distance_sum=0
distance_min=100
distance_max=100 # maybe larger
min_num=0
for k in range(possi_num):
for i in range(self.uav_num-1):
distance[i]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,num_array_possi[k][i]])
distance_mid_max=max(distance)
if distance_mid_max<distance_max:
distance_max=distance_mid_max
min_num=k
for k in range(possi_num):
for i in range(self.uav_num-1):
distance[i]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,num_array_possi[k][i]])
distance_sum=distance_sum+distance[i]
if distance_sum>distance_min:
break
if distance_min>distance_sum:
distance_min=distance_sum
#print(distance_min)
min_num=k
distance_sum=0
change_id=num_array_possi[min_num]
'''
change_id=[i + 1 for i in change_id]
#print (change_id)
for i in range(0,self.uav_num-1):
@ -296,207 +219,14 @@ class Follower:
new_formation[:,i]=position[:,j]
return new_formation
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
def get_L_matrix(self,rel_posi):
c_num=int((self.uav_num-1)/2)
min_num_index_list = [0]*c_num
comm=[[]for i in range (self.uav_num)]
w=numpy.ones((self.uav_num,self.uav_num))*0
nodes_next=[]
node_flag = [self.uav_num-1]
node_mid_flag=[]
#central-station control
def get_L_central_matrix(self):
rel_d=[0]*(self.uav_num-1)
for i in range(0,self.uav_num-1):
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
c=numpy.copy(rel_d)
c.sort()
count=0
for j in range(0,c_num):
for i in range(0,self.uav_num-1):
if rel_d[i]==c[j]:
if not i in node_mid_flag:
min_num_index_list[count]=i
node_mid_flag.append(i)
count=count+1
if count==c_num:
break
if count==c_num:
break
for j in range(0,c_num):
nodes_next.append(min_num_index_list[j])
comm[self.uav_num-1].append(min_num_index_list[j])
size_=len(node_flag)
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
next_node= nodes_next[0]
nodes_next=nodes_next[1:]
min_num_index_list = [0]*c_num
node_mid_flag=[]
rel_d=[0]*(self.uav_num-1)
for i in range(0,self.uav_num-1):
if i==next_node or i in node_flag:
rel_d[i]=2000
else:
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
c=numpy.copy(rel_d)
c.sort()
count=0
for j in range(0,c_num):
for i in range(0,self.uav_num-1):
if rel_d[i]==c[j]:
if not i in node_mid_flag:
min_num_index_list[count]=i
node_mid_flag.append(i)
count=count+1
if count==c_num:
break
if count==c_num:
break
node_flag.append(next_node)
size_=len(node_flag)
for j in range(0,c_num):
if min_num_index_list[j] in node_flag:
nodes_next=nodes_next
else:
if min_num_index_list[j] in nodes_next:
nodes_next=nodes_next
else:
nodes_next.append(min_num_index_list[j])
comm[next_node].append(min_num_index_list[j])
for i in range (0,self.uav_num):
for j in range(0,self.uav_num-1):
if i==0:
if j in comm[self.uav_num-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
else:
if j in comm[i-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
L=w
for i in range (0,self.uav_num):
L[i][i]=-sum(w[i])
return L
'''
def get_L_matrix(self, rel_posi):
#假设无论多少UAV，都假设尽可能3层通信（叶子节点除外）
c_num=int((self.uav_num-1)/3)
comm=[[]for i in range (self.uav_num)]
w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
nodes_next=[]
node_flag = [self.uav_num-1]
rel_d=[0]*(self.uav_num-1)
# 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
# 计算第一层通信（leader）：获得离自己最近的三台无人机编号
for i in range(0,self.uav_num-1):
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
min_num_index_list=list(min_num_index_list)
#leader 连接的无人机编号：
comm[self.uav_num-1]=min_num_index_list
nodes_next.extend(comm[self.uav_num-1])
size_=len(node_flag)
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
next_node= nodes_next[0]
nodes_next=nodes_next[1:]
rel_d=[0]*(self.uav_num-1)
for i in range(0,self.uav_num-1):
if i==next_node or i in node_flag:
rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
else:
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
min_num_index_list=list(min_num_index_list)
node_flag.append(next_node)
size_=len(node_flag)
for j in range(0,c_num):
if min_num_index_list[j] in node_flag:
nodes_next=nodes_next
else:
if min_num_index_list[j] in nodes_next:
nodes_next=nodes_next
else:
nodes_next.append(min_num_index_list[j])
comm[next_node].append(min_num_index_list[j])
# comm为每个uav连接其他uav的编号，其中数组的最后一行为leader
#print (comm)
#leader是拉普拉斯矩阵的第一行，为0
#第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
for i in range (0,self.uav_num):
for j in range(0,self.uav_num-1):
if i==0:
if j in comm[self.uav_num-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
else:
if j in comm[i-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
L=w #定义拉普拉斯矩阵
for i in range (0,self.uav_num):
L[i][i]=-sum(w[i])
#print (L)
L=numpy.zeros((self.uav_num,self.uav_num))
for i in range(1,self.uav_num):
L[i][0]=1
L[i][i]=-1
return L
'''
if __name__ == '__main__':
follower = Follower(sys.argv[1],int(sys.argv[2]),int(sys.argv[3]))

2
coordination/formation_demo/leader.py
View File

@ -12,6 +12,8 @@ if sys.argv[2] == '6':
from formation_dict import formation_dict_6 as formation_dict
elif sys.argv[2] == '9':
from formation_dict import formation_dict_9 as formation_dict
elif sys.argv[2] == '18':
from formation_dict import formation_dict_18 as formation_dict
class Leader:

2
coordination/formation_demo/run_formation.sh
View File

@ -3,6 +3,6 @@ python leader.py $1 $2 &
uav_num=1
while(( $uav_num< $2 ))
do
python follower_accel_control.py $1 $uav_num $2&
python follower.py $1 $uav_num $2&
let "uav_num++"
done

2
coordination/formation_demo/stop_formation.sh
View File

@ -1,2 +1,2 @@
kill -9 $(ps -ef|grep follower_accel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep follower.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

714
coordination/launch_generator/multi_vehicle.launch
View File

@ -18,8 +18,8 @@
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- rover_0 -->
<group ns="rover_0">
<!-- plane_0 -->
<group ns="plane_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="0"/>
@ -32,97 +32,10 @@
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="24562"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- rover_1 -->
<group ns="rover_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="24564"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- rover_2 -->
<group ns="rover_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="24566"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- plane_0 -->
<group ns="plane_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="4"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="plane"/>
<arg name="sdf" value="plane"/>
<arg name="mavlink_udp_port" value="24568"/>
<arg name="mavlink_tcp_port" value="4564"/>
<arg name="mavlink_udp_port" value="24562"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -137,21 +50,21 @@
<!-- plane_1 -->
<group ns="plane_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="5"/>
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="plane"/>
<arg name="sdf" value="plane"/>
<arg name="mavlink_udp_port" value="24570"/>
<arg name="mavlink_tcp_port" value="4565"/>
<arg name="mavlink_udp_port" value="24564"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -166,21 +79,21 @@
<!-- plane_2 -->
<group ns="plane_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="6"/>
<arg name="ID" value="3"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="12"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="plane"/>
<arg name="sdf" value="plane"/>
<arg name="mavlink_udp_port" value="24572"/>
<arg name="mavlink_tcp_port" value="4566"/>
<arg name="mavlink_udp_port" value="24566"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -195,21 +108,21 @@
<!-- typhoon_h480_0 -->
<group ns="typhoon_h480_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="7"/>
<arg name="ID" value="4"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="x" value="3"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480_stereo"/>
<arg name="mavlink_udp_port" value="24574"/>
<arg name="mavlink_tcp_port" value="4567"/>
<arg name="mavlink_udp_port" value="24568"/>
<arg name="mavlink_tcp_port" value="4564"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -224,21 +137,21 @@
<!-- typhoon_h480_1 -->
<group ns="typhoon_h480_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<arg name="ID" value="5"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="12"/>
<arg name="x" value="3"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480_stereo"/>
<arg name="mavlink_udp_port" value="24576"/>
<arg name="mavlink_tcp_port" value="4568"/>
<arg name="mavlink_udp_port" value="24570"/>
<arg name="mavlink_tcp_port" value="4565"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -253,21 +166,21 @@
<!-- typhoon_h480_2 -->
<group ns="typhoon_h480_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<arg name="ID" value="6"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="15"/>
<arg name="x" value="3"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480_stereo"/>
<arg name="mavlink_udp_port" value="24578"/>
<arg name="mavlink_tcp_port" value="4569"/>
<arg name="mavlink_udp_port" value="24572"/>
<arg name="mavlink_tcp_port" value="4566"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -282,12 +195,99 @@
<!-- solo_0 -->
<group ns="solo_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="10"/>
<arg name="ID" value="7"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="mavlink_udp_port" value="24574"/>
<arg name="mavlink_tcp_port" value="4567"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_1 -->
<group ns="solo_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="mavlink_udp_port" value="24576"/>
<arg name="mavlink_tcp_port" value="4568"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_2 -->
<group ns="solo_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="mavlink_udp_port" value="24578"/>
<arg name="mavlink_tcp_port" value="4569"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_3 -->
<group ns="solo_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="10"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:14550@127.0.0.1:34590"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="x" value="6"/>
<arg name="y" value="12"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
@ -308,15 +308,15 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_1 -->
<group ns="solo_1">
<!-- solo_4 -->
<group ns="solo_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="11"/>
<arg name="ID_in_group" value="1"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:14551@127.0.0.1:34592"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="x" value="6"/>
<arg name="y" value="15"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
@ -337,15 +337,15 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_2 -->
<group ns="solo_2">
<!-- solo_5 -->
<group ns="solo_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="12"/>
<arg name="ID_in_group" value="2"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:14552@127.0.0.1:34594"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="x" value="6"/>
<arg name="y" value="18"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
@ -366,22 +366,22 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_3 -->
<group ns="solo_3">
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="13"/>
<arg name="ID_in_group" value="3"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14553@127.0.0.1:34596"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="21"/>
<arg name="x" value="9"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24586"/>
<arg name="mavlink_tcp_port" value="4573"/>
<arg name="ID" value="$(arg ID)"/>
@ -395,22 +395,22 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_4 -->
<group ns="solo_4">
<!-- iris_1 -->
<group ns="iris_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="14"/>
<arg name="ID_in_group" value="4"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14554@127.0.0.1:34598"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="24"/>
<arg name="x" value="9"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24588"/>
<arg name="mavlink_tcp_port" value="4574"/>
<arg name="ID" value="$(arg ID)"/>
@ -424,22 +424,22 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- solo_5 -->
<group ns="solo_5">
<!-- iris_2 -->
<group ns="iris_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="15"/>
<arg name="ID_in_group" value="5"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14555@127.0.0.1:34600"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="27"/>
<arg name="x" value="9"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="solo"/>
<arg name="sdf" value="solo_stereo_camera"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24590"/>
<arg name="mavlink_tcp_port" value="4575"/>
<arg name="ID" value="$(arg ID)"/>
@ -453,16 +453,16 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_0 -->
<group ns="iris_0">
<!-- iris_3 -->
<group ns="iris_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="16"/>
<arg name="ID_in_group" value="0"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:14556@127.0.0.1:34602"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="15"/>
<arg name="x" value="9"/>
<arg name="y" value="12"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -482,16 +482,16 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_1 -->
<group ns="iris_1">
<!-- iris_4 -->
<group ns="iris_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="17"/>
<arg name="ID_in_group" value="1"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:14557@127.0.0.1:34604"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="18"/>
<arg name="x" value="9"/>
<arg name="y" value="15"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -511,16 +511,16 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_2 -->
<group ns="iris_2">
<!-- iris_5 -->
<group ns="iris_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="18"/>
<arg name="ID_in_group" value="2"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:14558@127.0.0.1:34606"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="21"/>
<arg name="x" value="9"/>
<arg name="y" value="18"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -539,112 +539,25 @@
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_3 -->
<group ns="iris_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="19"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:14559@127.0.0.1:34608"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="24"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24598"/>
<arg name="mavlink_tcp_port" value="4579"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_4 -->
<group ns="iris_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="20"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:14560@127.0.0.1:34610"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="27"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24600"/>
<arg name="mavlink_tcp_port" value="4580"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_5 -->
<group ns="iris_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="21"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:14561@127.0.0.1:34612"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="30"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24602"/>
<arg name="mavlink_tcp_port" value="4581"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tiltrotor_0 -->
<group ns="tiltrotor_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="22"/>
<arg name="ID" value="19"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14562@127.0.0.1:34614"/>
<arg name="fcu_url" default="udp://:14559@127.0.0.1:34608"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="18"/>
<arg name="x" value="12"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tiltrotor"/>
<arg name="sdf" value="tiltrotor"/>
<arg name="mavlink_udp_port" value="24604"/>
<arg name="mavlink_tcp_port" value="4582"/>
<arg name="mavlink_udp_port" value="24598"/>
<arg name="mavlink_tcp_port" value="4579"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -659,21 +572,21 @@
<!-- tiltrotor_1 -->
<group ns="tiltrotor_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="23"/>
<arg name="ID" value="20"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14563@127.0.0.1:34616"/>
<arg name="fcu_url" default="udp://:14560@127.0.0.1:34610"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="21"/>
<arg name="x" value="12"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tiltrotor"/>
<arg name="sdf" value="tiltrotor"/>
<arg name="mavlink_udp_port" value="24606"/>
<arg name="mavlink_tcp_port" value="4583"/>
<arg name="mavlink_udp_port" value="24600"/>
<arg name="mavlink_tcp_port" value="4580"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
@ -688,282 +601,21 @@
<!-- tiltrotor_2 -->
<group ns="tiltrotor_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="24"/>
<arg name="ID" value="21"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14564@127.0.0.1:34618"/>
<arg name="fcu_url" default="udp://:14561@127.0.0.1:34612"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="24"/>
<arg name="x" value="12"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tiltrotor"/>
<arg name="sdf" value="tiltrotor"/>
<arg name="mavlink_udp_port" value="24608"/>
<arg name="mavlink_tcp_port" value="4584"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_0 -->
<group ns="tailsitter_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="25"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14565@127.0.0.1:34620"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="21"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24610"/>
<arg name="mavlink_tcp_port" value="4585"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_1 -->
<group ns="tailsitter_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="26"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14566@127.0.0.1:34622"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="24"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24612"/>
<arg name="mavlink_tcp_port" value="4586"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_2 -->
<group ns="tailsitter_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="27"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14567@127.0.0.1:34624"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="27"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24614"/>
<arg name="mavlink_tcp_port" value="4587"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_3 -->
<group ns="tailsitter_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="28"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:14568@127.0.0.1:34626"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="30"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24616"/>
<arg name="mavlink_tcp_port" value="4588"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_4 -->
<group ns="tailsitter_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="29"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:14569@127.0.0.1:34628"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="33"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24618"/>
<arg name="mavlink_tcp_port" value="4589"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- tailsitter_5 -->
<group ns="tailsitter_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="30"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:14570@127.0.0.1:34630"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="36"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="tailsitter"/>
<arg name="sdf" value="tailsitter"/>
<arg name="mavlink_udp_port" value="24620"/>
<arg name="mavlink_tcp_port" value="4590"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- standard_vtol_0 -->
<group ns="standard_vtol_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="31"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:14571@127.0.0.1:34632"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="24"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="standard_vtol"/>
<arg name="sdf" value="standard_vtol"/>
<arg name="mavlink_udp_port" value="24622"/>
<arg name="mavlink_tcp_port" value="4591"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- standard_vtol_1 -->
<group ns="standard_vtol_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="32"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:14572@127.0.0.1:34634"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="27"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="standard_vtol"/>
<arg name="sdf" value="standard_vtol"/>
<arg name="mavlink_udp_port" value="24624"/>
<arg name="mavlink_tcp_port" value="4592"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- standard_vtol_2 -->
<group ns="standard_vtol_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="33"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:14573@127.0.0.1:34636"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="30"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="standard_vtol"/>
<arg name="sdf" value="standard_vtol"/>
<arg name="mavlink_udp_port" value="24626"/>
<arg name="mavlink_tcp_port" value="4593"/>
<arg name="mavlink_udp_port" value="24602"/>
<arg name="mavlink_tcp_port" value="4581"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>

BIN
image/rgbdslam.gif Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 901 KiB

8
motion_planning/launch/2d_motion_planning.launch
View File

@ -1,18 +1,20 @@
<launch>
<node pkg="tf" type="static_transform_publisher" name="odom_to_amp"
args="0.0 0.0 0 0.0 0.0 0.0 /odom /map 40" />
<!-- Arguments -->
<arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
<arg name="move_forward_only" default="false"/>
<arg name="open_rviz" default="true"/>
<!-- Map server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg motion_planning)/map/indoor3.yaml"/>
<node pkg="map_server" name="map_server" type="map_server" args="$(arg motion_planning)/map//indoor3.yaml"/>
<!-- move_base -->
<include file="$(find px4)/launch/move_base.launch">
<arg name="motion_planning" value="$(arg motion_planning)"/>
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
<arg name="cmd_vel_topic" default="/xtdrone/cmd_vel_flu" />
<arg name="odom_topic" default="/mavros/local_position/odom" />
<arg name="cmd_vel_topic" default="/xtdrone/iris_0/cmd_vel_flu" />
<arg name="odom_topic" default="/iris_0/mavros/local_position/odom" />
</include>
<!-- rviz -->

6
motion_planning/param/base_local_planner_params.yaml
View File

@ -1,11 +1,11 @@
TrajectoryPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.08
max_vel_x: 0.3
min_vel_x: 0.02
max_vel_theta: 0.3
min_vel_theta: -0.3
max_vel_theta: 0.1
min_vel_theta: -0.1
min_in_place_vel_theta: 1.0
acc_lim_x: 1.0

4
motion_planning/param/costmap_common_params.yaml
View File

@ -7,5 +7,5 @@ inflation_radius: 1.0
cost_scaling_factor: 2.0
map_type: costmap
observation_sources: scan
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
observation_sources: iris_0/scan
iris_0/scan: {sensor_frame: iris_0/laser_2d, data_type: LaserScan, topic: iris_0/scan, marking: true, clearing: true}

16
motion_planning/param/dwa_local_planner_params.yaml
View File

@ -1,18 +1,18 @@
DWAPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.1
min_vel_x: -0.1
max_vel_x: 0.2
min_vel_x: -0.2
max_vel_y: 0.1
min_vel_y: -0.1
max_vel_y: 0.2
min_vel_y: -0.2
# The velocity when robot is moving in a straight line
max_trans_vel: 0.1
min_trans_vel: 0.05
max_trans_vel: 0.2
min_trans_vel: 0.01
max_rot_vel: 0.5
min_rot_vel: 0.1
max_rot_vel: 0.1
min_rot_vel: 0.01
acc_lim_x: 1
acc_lim_y: 0.0

2
motion_planning/param/global_costmap_params.yaml
View File

@ -1,6 +1,6 @@
global_costmap:
global_frame: map
robot_base_frame: base_link
robot_base_frame: iris_0/base_link
update_frequency: 10.0
publish_frequency: 10.0

2
motion_planning/param/local_costmap_params.yaml
View File

@ -1,6 +1,6 @@
local_costmap:
global_frame: map
robot_base_frame: base_link
robot_base_frame: iris_0/base_link
update_frequency: 10.0
publish_frequency: 10.0

2
motion_planning/rviz/2d_motion_planning.rviz
View File

@ -90,7 +90,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.1
Style: Flat Squares
Topic: /scan
Topic: /iris_0/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true

2
sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml
View File

@ -1,7 +1,7 @@
subscribers:
camera_reading:
topic: /iris_0/stereo/camera/left/image_raw
topic: /iris_0/stereo_camera/left/image_raw
queue_size: 1
actions:

2
sensing/pose_ground_truth/get_local_pose.py
View File

@ -17,7 +17,7 @@ def odm_groundtruth_callback(msg, i):
multi_local_pose[i].header.frame_id = 'map'
multi_local_pose[i].pose = msg.pose.pose
multi_speed[i].header = msg.header
multi_speed[i].vector = msg.twist.linear
multi_speed[i].vector = msg.twist.twist.linear
if __name__ == '__main__':
rospy.init_node(vehicle_type+'get_pose_groundtruth')

3
sensing/slam/laser_slam/launch/matcher.launch
View File

@ -21,9 +21,12 @@
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
name="laser_scan_matcher_node" output="screen">
<param name="fixed_frame" value = "odom"/>
<param name="base_frame" value = "iris_0/base_link"/>
<param name="max_iterations" value="10"/>
<param name="use_imu" value="false"/>
<param name="use_odom" value="false"/>
<remap from="scan" to="iris_0/scan"/>
<remap from="pose2D" to="iris_0/pose2D"/>
</node>
</launch>

13
sensing/slam/laser_slam/launch/matcher_gmapping.launch
View File

@ -15,18 +15,21 @@ slam_gmapping
#### start the laser scan_matcher ##############################
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
name="laser_scan_matcher_node" output="screen">
<param name="fixed_frame" value = "map"/>
<param name="fixed_frame" value = "odom"/>
<param name="base_frame" value = "iris_0/base_link"/>
<param name="max_iterations" value="10"/>
<param name="use_imu" value="false"/>
<param name="use_odom" value="false"/>
<remap from="scan" to="iris_0/scan"/>
<remap from="pose2D" to="iris_0/pose2D"/>
</node>
#### start gmapping ############################################
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value='iris_0/base_link'/>
<param name="map_udpate_interval" value="1.0"/>
<param name="maxUrange" value="5.0"/>
<param name="sigma" value="0.1"/>
@ -55,6 +58,8 @@ slam_gmapping
<param name="llsamplestep" value="0.05"/>
<param name="lasamplerange" value="0.05"/>
<param name="lasamplestep" value="0.05"/>
<remap from="scan" to="iris_0/scan"/>
<remap from="pose2D" to="iris_0/pose2D"/>
</node>
</launch>

2
sensing/slam/laser_slam/rviz/matcher.rviz
View File

@ -77,7 +77,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.1
Style: Flat Squares
Topic: /scan
Topic: /iris_0/scan
Use Fixed Frame: true
Use rainbow: true
Value: true

2
sensing/slam/laser_slam/rviz/matcher_gmapping.rviz
View File

@ -79,7 +79,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.1
Style: Flat Squares
Topic: /scan
Topic: /iris_0/scan
Use Fixed Frame: true
Use rainbow: true
Value: true

105
sensing/slam/laser_slam/script/data_transfer.py
View File

@ -1,105 +0,0 @@
import rospy
from gazebo_msgs.srv import GetModelState
from geometry_msgs.msg import PoseStamped, Pose2D
from nav_msgs.msg import Odometry
import tf
import sys
rospy.init_node('laser_slam_data_transfer')
pose_pub = rospy.Publisher("/mavros/vision_pose/pose", PoseStamped, queue_size=2)
local_pose = PoseStamped()
local_pose.header.frame_id = 'map'
laser_scan = Pose2D()
def odm_groundtruth_callback(msg):
global local_pose
local_pose.header.stamp = msg.header.stamp
local_pose.pose.position.z = msg.pose.pose.position.z
def odm_aloam_callback(msg):
global local_pose
local_pose.header.stamp = msg.header.stamp
local_pose.pose = msg.pose.pose
def callback(data):
global laser_scan
laser_scan = data
def laser_scan_matcher():
global local_pose
pose2d_sub = rospy.Subscriber("/pose2D", Pose2D, callback)
rate = rospy.Rate(100)
while True:
local_pose.header.stamp = rospy.Time.now()
local_pose.pose.position.x = laser_scan.x
local_pose.pose.position.y = laser_scan.y
quaternion = tf.transformations.quaternion_from_euler(0, 0, laser_scan.theta)
local_pose.pose.orientation.x = quaternion[0]
local_pose.pose.orientation.y = quaternion[1]
local_pose.pose.orientation.z = quaternion[2]
local_pose.pose.orientation.w = quaternion[3]
rate.sleep()
def cartographer2D():
listener = tf.TransformListener()
global local_pose
rate = rospy.Rate(100)
while True:
try:
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
local_pose.pose.position.x = translation[0]
local_pose.pose.position.y = translation[1]
local_pose.pose.orientation.y = rotation[2]
local_pose.pose.orientation.z = rotation[3]
pose_pub.publish(local_pose)
rate.sleep()
def cartographer3D():
listener = tf.TransformListener()
global local_pose
rate = rospy.Rate(100)
while True:
try:
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
local_pose.pose.position.x = translation[0]
local_pose.pose.position.y = translation[1]
local_pose.pose.position.z = translation[2]
local_pose.pose.orientation.w = rotation[0]
local_pose.pose.orientation.x = rotation[1]
local_pose.pose.orientation.y = rotation[2]
local_pose.pose.orientation.z = rotation[3]
pose_pub.publish(local_pose)
rate.sleep()
def aloam():
global local_pose
rate = rospy.Rate(100)
while True:
pose_pub.publish(local_pose)
rate.sleep()
if __name__ == '__main__':
try:
laser_slam_type = sys.argv[1]
except:
print('You should choose from laser_scan_matcher, cartographer2D and cartographer3D.')
if laser_slam_type == 'laser_scan_matcher':
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/ground_truth/odom', Odometry, odm_groundtruth_callback)
laser_scan_matcher()
elif laser_slam_type == 'cartographer2D':
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/ground_truth/odom', Odometry, odm_groundtruth_callback)
cartographer2D()
elif laser_slam_type == 'cartographer3D':
cartographer3D()
elif laser_slam_type == 'aloam':
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
aloam()
else:
print('You should choose from laser_scan_matcher, cartographer2D and cartographer3D.')

66
sensing/slam/laser_slam/script/laser_transfer.py Executable file
View File

@ -0,0 +1,66 @@
import rospy
from gazebo_msgs.srv import GetModelState
from geometry_msgs.msg import PoseStamped, Pose2D
from nav_msgs.msg import Odometry
import tf
import sys
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
laser_slam_type = sys.argv[3]
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=2)
local_pose = PoseStamped()
local_pose.header.frame_id = 'map'
laser_scan = Pose2D()
def odm_groundtruth_callback(msg):
global local_pose
local_pose.header.stamp = msg.header.stamp
local_pose.pose.position.z = msg.pose.pose.position.z
def odm_aloam_callback(msg):
global local_pose
local_pose.header.stamp = msg.header.stamp
local_pose.pose = msg.pose.pose
def callback(data):
global laser_scan
laser_scan = data
def laser_scan_matcher():
global local_pose
pose2d_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/pose2D", Pose2D, callback)
rate = rospy.Rate(100)
while True:
local_pose.header.stamp = rospy.Time.now()
local_pose.pose.position.x = laser_scan.x
local_pose.pose.position.y = laser_scan.y
quaternion = tf.transformations.quaternion_from_euler(0, 0, laser_scan.theta)
local_pose.pose.orientation.x = quaternion[0]
local_pose.pose.orientation.y = quaternion[1]
local_pose.pose.orientation.z = quaternion[2]
local_pose.pose.orientation.w = quaternion[3]
pose_pub.publish(local_pose)
rate.sleep()
def aloam():
global local_pose
rate = rospy.Rate(100)
while True:
pose_pub.publish(local_pose)
rate.sleep()
if __name__ == '__main__':
if laser_slam_type == '2d':
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
laser_scan_matcher()
elif laser_slam_type == '3d':
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
aloam()
else:
print('input error')

16
sensing/slam/vio/VINS-Fusion/.gitignore vendored Normal file
View File

@ -0,0 +1,16 @@
*~
ex_calib_result.yaml
vins_result.csv
data_generator/
test/
test.launch
evaluate/
.vscode/
result.txt
result_global.txt
result_gps.txt
0*.txt
1*.txt
2*.txt
camera_camera_calib.yaml
camera_chessboard_data.dat

0
sensing/slam/vio/VINS-Mono/LICENCE → sensing/slam/vio/VINS-Fusion/LICENCE Executable file → Normal file
View File

176
sensing/slam/vio/VINS-Fusion/README.md Normal file
View File

@ -0,0 +1,176 @@
# VINS-Fusion
## An optimization-based multi-sensor state estimator
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/vins_logo.png" width = 55% height = 55% div align=left />
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/kitti.png" width = 34% height = 34% div align=center />
VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension of [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono), which supports multiple visual-inertial sensor types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS with GPS.
**Features:**
- multiple sensors support (stereo cameras / mono camera+IMU / stereo cameras+IMU)
- online spatial calibration (transformation between camera and IMU)
- online temporal calibration (time offset between camera and IMU)
- visual loop closure
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/kitti_rank.png" width = 80% height = 80% />
We are the **top** open-sourced stereo algorithm on [KITTI Odometry Benchmark](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) (12.Jan.2019).
**Authors:** [Tong Qin](http://www.qintonguav.com), Shaozu Cao, Jie Pan, [Peiliang Li](https://peiliangli.github.io/), and [Shaojie Shen](http://www.ece.ust.hk/ece.php/profile/facultydetail/eeshaojie) from the [Aerial Robotics Group](http://uav.ust.hk/), [HKUST](https://www.ust.hk/)
**Videos:**
<a href="https://www.youtube.com/embed/1qye82aW7nI" target="_blank"><img src="http://img.youtube.com/vi/1qye82aW7nI/0.jpg"
alt="VINS" width="320" height="240" border="10" /></a>
**Related Paper:** (paper is not exactly same with code)
* **Online Temporal Calibration for Monocular Visual-Inertial Systems**, Tong Qin, Shaojie Shen, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS, 2018), **best student paper award** [pdf](https://ieeexplore.ieee.org/abstract/document/8593603)
* **VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator**, Tong Qin, Peiliang Li, Shaojie Shen, IEEE Transactions on Robotics [pdf](https://ieeexplore.ieee.org/document/8421746/?arnumber=8421746&source=authoralert)
*If you use VINS-Fusion for your academic research, please cite our related papers. [bib](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/paper_bib.txt)*
## 1. Prerequisites
### 1.1 **Ubuntu** and **ROS**
Ubuntu 64-bit 16.04 or 18.04.
ROS Kinetic or Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
### 1.2. **Ceres Solver**
Follow [Ceres Installation](http://ceres-solver.org/installation.html).
## 2. Build VINS-Fusion
Clone the repository and catkin_make:
```
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
```
(if you fail in this step, try to find another computer with clean system or reinstall Ubuntu and ROS)
## 3. EuRoC Example
Download [EuRoC MAV Dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) to YOUR_DATASET_FOLDER. Take MH_01 for example, you can run VINS-Fusion with three sensor types (monocular camera + IMU, stereo cameras + IMU and stereo cameras).
Open four terminals, run vins odometry, visual loop closure(optional), rviz and play the bag file respectively.
Green path is VIO odometry; red path is odometry under visual loop closure.
### 3.1 Monocualr camera + IMU
```
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```
### 3.2 Stereo cameras + IMU
```
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```
### 3.3 Stereo cameras
```
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/euroc.gif" width = 430 height = 240 />
## 4. KITTI Example
### 4.1 KITTI Odometry (Stereo)
Download [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) to YOUR_DATASET_FOLDER. Take sequences 00 for example,
Open two terminals, run vins and rviz respectively.
(We evaluated odometry on KITTI benchmark without loop closure funtion)
```
roslaunch vins vins_rviz.launch
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml
rosrun vins kitti_odom_test ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/
```
### 4.2 KITTI GPS Fusion (Stereo + GPS)
Download [KITTI raw dataset](http://www.cvlibs.net/datasets/kitti/raw_data.php) to YOUR_DATASET_FOLDER. Take [2011_10_03_drive_0027_synced](https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_10_03_drive_0027/2011_10_03_drive_0027_sync.zip) for example.
Open three terminals, run vins, global fusion and rviz respectively.
Green path is VIO odometry; blue path is odometry under GPS global fusion.
```
roslaunch vins vins_rviz.launch
rosrun vins kitti_gps_test ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/
rosrun global_fusion global_fusion_node
```
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/kitti.gif" width = 430 height = 240 />
## 5. VINS-Fusion on car demonstration
Download [car bag](https://drive.google.com/open?id=10t9H1u8pMGDOI6Q2w2uezEq5Ib-Z8tLz) to YOUR_DATASET_FOLDER.
Open four terminals, run vins odometry, visual loop closure(optional), rviz and play the bag file respectively.
Green path is VIO odometry; red path is odometry under visual loop closure.
```
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml
rosbag play YOUR_DATASET_FOLDER/car.bag
```
<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/car_gif.gif" width = 430 height = 240 />
## 6. Run with your devices
VIO is not only a software algorithm, it heavily relies on hardware quality. For beginners, we recommend you to run VIO with professional equipment, which contains global shutter cameras and hardware synchronization.
### 6.1 Configuration file
Write a config file for your device. You can take config files of EuRoC and KITTI as the example.
### 6.2 Camera calibration
VINS-Fusion support several camera models (pinhole, mei, equidistant). You can use [camera model](https://github.com/hengli/camodocal) to calibrate your cameras. We put some example data under /camera_models/calibrationdata to tell you how to calibrate.
```
cd ~/catkin_ws/src/VINS-Fusion/camera_models/camera_calib_example/
rosrun camera_models Calibrations -w 12 -h 8 -s 80 -i calibrationdata --camera-model pinhole
```
## 7. Docker Support
To further facilitate the building process, we add docker in our code. Docker environment is like a sandbox, thus makes our code environment-independent. To run with docker, first make sure [ros](http://wiki.ros.org/ROS/Installation) and [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) are installed on your machine. Then add your account to `docker` group by `sudo usermod -aG docker $YOUR_USER_NAME`. **Relaunch the terminal or logout and re-login if you get `Permission denied` error**, type:
```
cd ~/catkin_ws/src/VINS-Fusion/docker
make build
```
Note that the docker building process may take a while depends on your network and machine. After VINS-Fusion successfully built, you can run vins estimator with script `run.sh`.
Script `run.sh` can take several flags and arguments. Flag `-k` means KITTI, `-l` represents loop fusion, and `-g` stands for global fusion. You can get the usage details by `./run.sh -h`. Here are some examples with this script:
```
# Euroc Monocualr camera + IMU
./run.sh ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
# Euroc Stereo cameras + IMU with loop fusion
./run.sh -l ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
# KITTI Odometry (Stereo)
./run.sh -k ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/
# KITTI Odometry (Stereo) with loop fusion
./run.sh -kl ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/
# KITTI GPS Fusion (Stereo + GPS)
./run.sh -kg ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/
```
In Euroc cases, you need open another terminal and play your bag file. If you need modify the code, simply re-run `./run.sh` with proper auguments after your changes.
## 8. Acknowledgements
We use [ceres solver](http://ceres-solver.org/) for non-linear optimization and [DBoW2](https://github.com/dorian3d/DBoW2) for loop detection, a generic [camera model](https://github.com/hengli/camodocal) and [GeographicLib](https://geographiclib.sourceforge.io/).
## 9. License
The source code is released under [GPLv3](http://www.gnu.org/licenses/) license.
We are still working on improving the code reliability. For any technical issues, please contact Tong Qin <qintonguavATgmail.com>.
For commercial inquiries, please contact Shaojie Shen <eeshaojieATust.hk>.

15
sensing/slam/vio/VINS-Mono/camera_model/CMakeLists.txt → sensing/slam/vio/VINS-Fusion/camera_models/CMakeLists.txt Executable file → Normal file
View File

@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
project(camera_model)
project(camera_models)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
@ -22,7 +22,7 @@ include_directories(${CERES_INCLUDE_DIRS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES camera_model
LIBRARIES camera_models
CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
@ -33,8 +33,7 @@ include_directories(
include_directories("include")
add_executable(Calibration
add_executable(Calibrations
src/intrinsic_calib.cc
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
@ -42,6 +41,7 @@ add_executable(Calibration
src/camera_models/CameraFactory.cc
src/camera_models/CostFunctionFactory.cc
src/camera_models/PinholeCamera.cc
src/camera_models/PinholeFullCamera.cc
src/camera_models/CataCamera.cc
src/camera_models/EquidistantCamera.cc
src/camera_models/ScaramuzzaCamera.cc
@ -49,13 +49,14 @@ add_executable(Calibration
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
add_library(camera_model
add_library(camera_models
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
src/camera_models/Camera.cc
src/camera_models/CameraFactory.cc
src/camera_models/CostFunctionFactory.cc
src/camera_models/PinholeCamera.cc
src/camera_models/PinholeFullCamera.cc
src/camera_models/CataCamera.cc
src/camera_models/EquidistantCamera.cc
src/camera_models/ScaramuzzaCamera.cc
@ -63,5 +64,5 @@ add_library(camera_model
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
target_link_libraries(Calibrations ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
target_link_libraries(camera_models ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})

0
sensing/slam/vio/VINS-Mono/ar_demo/cmake/FindEigen.cmake → sensing/slam/vio/VINS-Fusion/camera_models/cmake/FindEigen.cmake Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/calib/CameraCalibration.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/calib/CameraCalibration.h Executable file → Normal file
View File

148
sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/Camera.h Normal file
View File

@ -0,0 +1,148 @@
#ifndef CAMERA_H
#define CAMERA_H
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <vector>
namespace camodocal
{
class Camera
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum ModelType
{
KANNALA_BRANDT,
MEI,
PINHOLE,
PINHOLE_FULL,
SCARAMUZZA
};
class Parameters
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Parameters( ModelType modelType );
Parameters( ModelType modelType, const std::string& cameraName, int w, int h );
ModelType& modelType( void );
std::string& cameraName( void );
int& imageWidth( void );
int& imageHeight( void );
ModelType modelType( void ) const;
const std::string& cameraName( void ) const;
int imageWidth( void ) const;
int imageHeight( void ) const;
int nIntrinsics( void ) const;
virtual bool readFromYamlFile( const std::string& filename ) = 0;
virtual void writeToYamlFile( const std::string& filename ) const = 0;
protected:
ModelType m_modelType;
int m_nIntrinsics;
std::string m_cameraName;
int m_imageWidth;
int m_imageHeight;
};
virtual ModelType modelType( void ) const = 0;
virtual const std::string& cameraName( void ) const = 0;
virtual int imageWidth( void ) const = 0;
virtual int imageHeight( void ) const = 0;
virtual cv::Mat& mask( void );
virtual const cv::Mat& mask( void ) const;
virtual void estimateIntrinsics( const cv::Size& boardSize,
const std::vector< std::vector< cv::Point3f > >& objectPoints,
const std::vector< std::vector< cv::Point2f > >& imagePoints )
= 0;
virtual void estimateExtrinsics( const std::vector< cv::Point3f >& objectPoints,
const std::vector< cv::Point2f >& imagePoints,
cv::Mat& rvec,
cv::Mat& tvec ) const;
// Lift points from the image plane to the sphere
virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0;
//%output P
// Lift points from the image plane to the projective space
virtual void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0;
//%output P
// Projects 3D points to the image plane (Pi function)
virtual void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const = 0;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
// virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
// Eigen::Matrix<double,2,3>& J) const = 0;
//%output p
//%output J
virtual void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const = 0;
//%output p
// virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0)
// const = 0;
virtual cv::Mat initUndistortRectifyMap( cv::Mat& map1,
cv::Mat& map2,
float fx = -1.0f,
float fy = -1.0f,
cv::Size imageSize = cv::Size( 0, 0 ),
float cx = -1.0f,
float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const = 0;
virtual int parameterCount( void ) const = 0;
virtual void readParameters( const std::vector< double >& parameters ) = 0;
virtual void writeParameters( std::vector< double >& parameters ) const = 0;
virtual void writeParametersToYamlFile( const std::string& filename ) const = 0;
virtual std::string parametersToString( void ) const = 0;
/**
* \brief Calculates the reprojection distance between points
*
* \param P1 first 3D point coordinates
* \param P2 second 3D point coordinates
* \return euclidean distance in the plane
*/
double reprojectionDist( const Eigen::Vector3d& P1, const Eigen::Vector3d& P2 ) const;
double reprojectionError( const std::vector< std::vector< cv::Point3f > >& objectPoints,
const std::vector< std::vector< cv::Point2f > >& imagePoints,
const std::vector< cv::Mat >& rvecs,
const std::vector< cv::Mat >& tvecs,
cv::OutputArray perViewErrors = cv::noArray( ) ) const;
double reprojectionError( const Eigen::Vector3d& P,
const Eigen::Quaterniond& camera_q,
const Eigen::Vector3d& camera_t,
const Eigen::Vector2d& observed_p ) const;
void projectPoints( const std::vector< cv::Point3f >& objectPoints,
const cv::Mat& rvec,
const cv::Mat& tvec,
std::vector< cv::Point2f >& imagePoints ) const;
protected:
cv::Mat m_mask;
};
typedef boost::shared_ptr< Camera > CameraPtr;
typedef boost::shared_ptr< const Camera > CameraConstPtr;
}
#endif

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/CameraFactory.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/CameraFactory.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/CataCamera.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/CataCamera.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/CostFunctionFactory.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/CostFunctionFactory.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/EquidistantCamera.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/EquidistantCamera.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/PinholeCamera.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/PinholeCamera.h Executable file → Normal file
View File

274
sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/PinholeFullCamera.h Normal file
View File

@ -0,0 +1,274 @@
#ifndef PinholeFullCAMERA_H
#define PinholeFullCAMERA_H
#include <opencv2/core/core.hpp>
#include <string>
#include "Camera.h"
#include "ceres/rotation.h"
namespace camodocal
{
class PinholeFullCamera : public Camera
{
public:
class Parameters : public Camera::Parameters
{
public:
Parameters( );
Parameters( const std::string& cameraName,
int w,
int h,
double k1,
double k2,
double k3,
double k4,
double k5,
double k6,
double p1,
double p2,
double fx,
double fy,
double cx,
double cy );
double& k1( void );
double& k2( void );
double& k3( void );
double& k4( void );
double& k5( void );
double& k6( void );
double& p1( void );
double& p2( void );
double& fx( void );
double& fy( void );
double& cx( void );
double& cy( void );
double xi( void ) const;
double k1( void ) const;
double k2( void ) const;
double k3( void ) const;
double k4( void ) const;
double k5( void ) const;
double k6( void ) const;
double p1( void ) const;
double p2( void ) const;
double fx( void ) const;
double fy( void ) const;
double cx( void ) const;
double cy( void ) const;
bool readFromYamlFile( const std::string& filename );
void writeToYamlFile( const std::string& filename ) const;
Parameters& operator=( const Parameters& other );
friend std::ostream& operator<<( std::ostream& out, const Parameters& params );
private:
double m_k1;
double m_k2;
double m_p1;
double m_p2;
double m_fx;
double m_fy;
double m_cx;
double m_cy;
double m_k3;
double m_k4;
double m_k5;
double m_k6;
};
PinholeFullCamera( );
/**
* \brief Constructor from the projection model parameters
*/
PinholeFullCamera( const std::string& cameraName,
int imageWidth,
int imageHeight,
double k1,
double k2,
double k3,
double k4,
double k5,
double k6,
double p1,
double p2,
double fx,
double fy,
double cx,
double cy );
/**
* \brief Constructor from the projection model parameters
*/
PinholeFullCamera( const Parameters& params );
Camera::ModelType modelType( void ) const;
const std::string& cameraName( void ) const;
int imageWidth( void ) const;
int imageHeight( void ) const;
cv::Size imageSize( ) const { return cv::Size( imageWidth( ), imageHeight( ) ); }
cv::Point2f getPrinciple( ) const
{
return cv::Point2f( mParameters.cx( ), mParameters.cy( ) );
}
void estimateIntrinsics( const cv::Size& boardSize,
const std::vector< std::vector< cv::Point3f > >& objectPoints,
const std::vector< std::vector< cv::Point2f > >& imagePoints );
void setInitIntrinsics( const std::vector< std::vector< cv::Point3f > >& objectPoints,
const std::vector< std::vector< cv::Point2f > >& imagePoints )
{
Parameters params = getParameters( );
params.k1( ) = 0.0;
params.k2( ) = 0.0;
params.k3( ) = 0.0;
params.k4( ) = 0.0;
params.k5( ) = 0.0;
params.k6( ) = 0.0;
params.p1( ) = 0.0;
params.p2( ) = 0.0;
double cx = params.imageWidth( ) / 2.0;
double cy = params.imageHeight( ) / 2.0;
params.cx( ) = cx;
params.cy( ) = cy;
params.fx( ) = 1200;
params.fy( ) = 1200;
setParameters( params );
}
// Lift points from the image plane to the sphere
virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const;
//%output P
void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const;
// Projects 3D points to the image plane (Pi function)
void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const;
//%output p
void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const;
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix< double, 2, 3 >& J ) const;
//%output p
//%output J
void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const;
//%output p
template< typename T >
static void spaceToPlane( const T* const params,
const T* const q,
const T* const t,
const Eigen::Matrix< T, 3, 1 >& P,
Eigen::Matrix< T, 2, 1 >& p );
void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u ) const;
void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J ) const;
void initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale = 1.0 ) const;
cv::Mat initUndistortRectifyMap( cv::Mat& map1,
cv::Mat& map2,
float fx = -1.0f,
float fy = -1.0f,
cv::Size imageSize = cv::Size( 0, 0 ),
float cx = -1.0f,
float cy = -1.0f,
cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const;
int parameterCount( void ) const;
const Parameters& getParameters( void ) const;
void setParameters( const Parameters& parameters );
void readParameters( const std::vector< double >& parameterVec );
void writeParameters( std::vector< double >& parameterVec ) const;
void writeParametersToYamlFile( const std::string& filename ) const;
std::string parametersToString( void ) const;
private:
Parameters mParameters;
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
bool m_noDistortion;
};
typedef boost::shared_ptr< PinholeFullCamera > PinholeFullCameraPtr;
typedef boost::shared_ptr< const PinholeFullCamera > PinholeFullCameraConstPtr;
template< typename T >
void
PinholeFullCamera::spaceToPlane( const T* const params,
const T* const q,
const T* const t,
const Eigen::Matrix< T, 3, 1 >& P,
Eigen::Matrix< T, 2, 1 >& p )
{
T P_w[3];
P_w[0] = T( P( 0 ) );
P_w[1] = T( P( 1 ) );
P_w[2] = T( P( 2 ) );
// Convert quaternion from Eigen convention (x, y, z, w)
// to Ceres convention (w, x, y, z)
T q_ceres[4] = { q[3], q[0], q[1], q[2] };
T P_c[3];
ceres::QuaternionRotatePoint( q_ceres, P_w, P_c );
P_c[0] += t[0];
P_c[1] += t[1];
P_c[2] += t[2];
// project 3D object point to the image plane
T k1 = params[0];
T k2 = params[1];
T k3 = params[2];
T k4 = params[3];
T k5 = params[4];
T k6 = params[5];
T p1 = params[6];
T p2 = params[7];
T fx = params[8];
T fy = params[9];
T alpha = T( 0 ); // cameraParams.alpha();
T cx = params[10];
T cy = params[11];
// Transform to model plane
T x = P_c[0] / P_c[2];
T y = P_c[1] / P_c[2];
// T z = P_c[2] / P_c[2];
T r2 = x * x + y * y;
T r4 = r2 * r2;
T r6 = r4 * r2;
T a1 = T( 2 ) * x * y;
T a2 = r2 + T( 2 ) * x * x;
T a3 = r2 + T( 2 ) * y * y;
T cdist = T( 1 ) + k1 * r2 + k2 * r4 + k3 * r6;
T icdist2 = T( 1. ) / ( T( 1 ) + k4 * r2 + k5 * r4 + k6 * r6 );
T xd0 = x * cdist * icdist2 + p1 * a1 + p2 * a2; // + k[8] * r2 + k[9] * r4;
T yd0 = y * cdist * icdist2 + p1 * a3 + p2 * a1; // + k[10] * r2 + k[11] * r4;
p( 0 ) = xd0 * fx + cx;
p( 1 ) = yd0 * fy + cy;
}
}
#endif

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/camera_models/ScaramuzzaCamera.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/camera_models/ScaramuzzaCamera.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/chessboard/Chessboard.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/chessboard/Chessboard.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/chessboard/ChessboardCorner.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/chessboard/ChessboardCorner.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/chessboard/ChessboardQuad.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/chessboard/ChessboardQuad.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/chessboard/Spline.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/chessboard/Spline.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/gpl/EigenUtils.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/gpl/EigenUtils.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/gpl/gpl.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/gpl/gpl.h Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/include/camodocal/sparse_graph/Transform.h → sensing/slam/vio/VINS-Fusion/camera_models/include/camodocal/sparse_graph/Transform.h Executable file → Normal file
View File

4
sensing/slam/vio/VINS-Mono/camera_model/package.xml → sensing/slam/vio/VINS-Fusion/camera_models/package.xml Executable file → Normal file
View File

@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package>
<name>camera_model</name>
<name>camera_models</name>
<version>0.0.0</version>
<description>The camera_model package</description>
<description>The camera_models package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->

0
sensing/slam/vio/VINS-Mono/camera_model/readme.md → sensing/slam/vio/VINS-Fusion/camera_models/readme.md Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/calib/CameraCalibration.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/calib/CameraCalibration.cc Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/camera_models/Camera.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/Camera.cc Executable file → Normal file
View File

190
sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/CameraFactory.cc Normal file
View File

@ -0,0 +1,190 @@
#include "camodocal/camera_models/CameraFactory.h"
#include <boost/algorithm/string.hpp>
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/EquidistantCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "camodocal/camera_models/PinholeFullCamera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"
#include "ceres/ceres.h"
namespace camodocal
{
boost::shared_ptr< CameraFactory > CameraFactory::m_instance;
CameraFactory::CameraFactory( ) {}
boost::shared_ptr< CameraFactory >
CameraFactory::instance( void )
{
if ( m_instance.get( ) == 0 )
{
m_instance.reset( new CameraFactory );
}
return m_instance;
}
CameraPtr
CameraFactory::generateCamera( Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize ) const
{
switch ( modelType )
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera( new EquidistantCamera );
EquidistantCamera::Parameters params = camera->getParameters( );
params.cameraName( ) = cameraName;
params.imageWidth( ) = imageSize.width;
params.imageHeight( ) = imageSize.height;
camera->setParameters( params );
return camera;
}
case Camera::PINHOLE:
{
PinholeCameraPtr camera( new PinholeCamera );
PinholeCamera::Parameters params = camera->getParameters( );
params.cameraName( ) = cameraName;
params.imageWidth( ) = imageSize.width;
params.imageHeight( ) = imageSize.height;
camera->setParameters( params );
return camera;
}
case Camera::PINHOLE_FULL:
{
PinholeFullCameraPtr camera( new PinholeFullCamera );
PinholeFullCamera::Parameters params = camera->getParameters( );
params.cameraName( ) = cameraName;
params.imageWidth( ) = imageSize.width;
params.imageHeight( ) = imageSize.height;
camera->setParameters( params );
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera( new OCAMCamera );
OCAMCamera::Parameters params = camera->getParameters( );
params.cameraName( ) = cameraName;
params.imageWidth( ) = imageSize.width;
params.imageHeight( ) = imageSize.height;
camera->setParameters( params );
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera( new CataCamera );
CataCamera::Parameters params = camera->getParameters( );
params.cameraName( ) = cameraName;
params.imageWidth( ) = imageSize.width;
params.imageHeight( ) = imageSize.height;
camera->setParameters( params );
return camera;
}
}
}
CameraPtr
CameraFactory::generateCameraFromYamlFile( const std::string& filename )
{
cv::FileStorage fs( filename, cv::FileStorage::READ );
if ( !fs.isOpened( ) )
{
return CameraPtr( );
}
Camera::ModelType modelType = Camera::MEI;
if ( !fs["model_type"].isNone( ) )
{
std::string sModelType;
fs["model_type"] >> sModelType;
if ( boost::iequals( sModelType, "kannala_brandt" ) )
{
modelType = Camera::KANNALA_BRANDT;
}
else if ( boost::iequals( sModelType, "mei" ) )
{
modelType = Camera::MEI;
}
else if ( boost::iequals( sModelType, "scaramuzza" ) )
{
modelType = Camera::SCARAMUZZA;
}
else if ( boost::iequals( sModelType, "pinhole" ) )
{
modelType = Camera::PINHOLE;
}
else if ( boost::iequals( sModelType, "PINHOLE_FULL" ) )
{
modelType = Camera::PINHOLE_FULL;
}
else
{
std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
return CameraPtr( );
}
}
switch ( modelType )
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera( new EquidistantCamera );
EquidistantCamera::Parameters params = camera->getParameters( );
params.readFromYamlFile( filename );
camera->setParameters( params );
return camera;
}
case Camera::PINHOLE:
{
PinholeCameraPtr camera( new PinholeCamera );
PinholeCamera::Parameters params = camera->getParameters( );
params.readFromYamlFile( filename );
camera->setParameters( params );
return camera;
}
case Camera::PINHOLE_FULL:
{
PinholeFullCameraPtr camera( new PinholeFullCamera );
PinholeFullCamera::Parameters params = camera->getParameters( );
params.readFromYamlFile( filename );
camera->setParameters( params );
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera( new OCAMCamera );
OCAMCamera::Parameters params = camera->getParameters( );
params.readFromYamlFile( filename );
camera->setParameters( params );
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera( new CataCamera );
CataCamera::Parameters params = camera->getParameters( );
params.readFromYamlFile( filename );
camera->setParameters( params );
return camera;
}
}
return CameraPtr( );
}
}

0
sensing/slam/vio/VINS-Mono/camera_model/src/camera_models/CataCamera.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/CataCamera.cc Executable file → Normal file
View File

1384
sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/CostFunctionFactory.cc Normal file
View File

File diff suppressed because it is too large Load Diff

0
sensing/slam/vio/VINS-Mono/camera_model/src/camera_models/EquidistantCamera.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/EquidistantCamera.cc Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/camera_models/PinholeCamera.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/PinholeCamera.cc Executable file → Normal file
View File

1000
sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/PinholeFullCamera.cc Normal file
View File

File diff suppressed because it is too large Load Diff

0
sensing/slam/vio/VINS-Mono/camera_model/src/camera_models/ScaramuzzaCamera.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/camera_models/ScaramuzzaCamera.cc Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/chessboard/Chessboard.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/chessboard/Chessboard.cc Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/gpl/EigenQuaternionParameterization.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/gpl/EigenQuaternionParameterization.cc Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/camera_model/src/gpl/gpl.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/gpl/gpl.cc Executable file → Normal file
View File

284
sensing/slam/vio/VINS-Fusion/camera_models/src/intrinsic_calib.cc Normal file
View File

@ -0,0 +1,284 @@
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iomanip>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "camodocal/calib/CameraCalibration.h"
#include "camodocal/chessboard/Chessboard.h"
#include "camodocal/gpl/gpl.h"
int
main( int argc, char** argv )
{
cv::Size boardSize;
float squareSize;
std::string inputDir;
std::string cameraModel;
std::string cameraName;
std::string prefix;
std::string fileExtension;
bool useOpenCV;
bool viewResults;
bool verbose;
//========= Handling Program options =========
boost::program_options::options_description desc( "Allowed options" );
desc.add_options( )( "help", "produce help message" )(
"width,w",
boost::program_options::value< int >( &boardSize.width )->default_value( 8 ),
"Number of inner corners on the chessboard pattern in x direction" )(
"height,h",
boost::program_options::value< int >( &boardSize.height )->default_value( 12 ),
"Number of inner corners on the chessboard pattern in y direction" )(
"size,s",
boost::program_options::value< float >( &squareSize )->default_value( 7.f ),
"Size of one square in mm" )( "input,i",
boost::program_options::value< std::string >( &inputDir )->default_value( "calibrationdata" ),
"Input directory containing chessboard images" )(
"prefix,p",
boost::program_options::value< std::string >( &prefix )->default_value( "left-" ),
"Prefix of images" )( "file-extension,e",
boost::program_options::value< std::string >( &fileExtension )->default_value( ".png" ),
"File extension of images" )(
"camera-model",
boost::program_options::value< std::string >( &cameraModel )->default_value( "mei" ),
"Camera model: kannala-brandt | mei | pinhole" )(
"camera-name",
boost::program_options::value< std::string >( &cameraName )->default_value( "camera" ),
"Name of camera" )( "opencv",
boost::program_options::bool_switch( &useOpenCV )->default_value( true ),
"Use OpenCV to detect corners" )(
"view-results",
boost::program_options::bool_switch( &viewResults )->default_value( false ),
"View results" )( "verbose,v",
boost::program_options::bool_switch( &verbose )->default_value( true ),
"Verbose output" );
boost::program_options::positional_options_description pdesc;
pdesc.add( "input", 1 );
boost::program_options::variables_map vm;
boost::program_options::store( boost::program_options::command_line_parser( argc, argv )
.options( desc )
.positional( pdesc )
.run( ),
vm );
boost::program_options::notify( vm );
if ( vm.count( "help" ) )
{
std::cout << desc << std::endl;
return 1;
}
if ( !boost::filesystem::exists( inputDir ) && !boost::filesystem::is_directory( inputDir ) )
{
std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl;
return 1;
}
camodocal::Camera::ModelType modelType;
if ( boost::iequals( cameraModel, "kannala-brandt" ) )
{
modelType = camodocal::Camera::KANNALA_BRANDT;
}
else if ( boost::iequals( cameraModel, "mei" ) )
{
modelType = camodocal::Camera::MEI;
}
else if ( boost::iequals( cameraModel, "pinhole" ) )
{
modelType = camodocal::Camera::PINHOLE;
}
else if ( boost::iequals( cameraModel, "pinhole_full" ) )
{
modelType = camodocal::Camera::PINHOLE_FULL;
}
else if ( boost::iequals( cameraModel, "scaramuzza" ) )
{
modelType = camodocal::Camera::SCARAMUZZA;
}
else
{
std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl;
return 1;
}
switch ( modelType )
{
case camodocal::Camera::KANNALA_BRANDT:
std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl;
break;
case camodocal::Camera::MEI:
std::cout << "# INFO: Camera model: Mei" << std::endl;
break;
case camodocal::Camera::PINHOLE:
std::cout << "# INFO: Camera model: Pinhole" << std::endl;
break;
case camodocal::Camera::PINHOLE_FULL:
std::cout << "# INFO: Camera model: PinholeFull" << std::endl;
break;
case camodocal::Camera::SCARAMUZZA:
std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl;
break;
}
// look for images in input directory
std::vector< std::string > imageFilenames;
boost::filesystem::directory_iterator itr;
for ( boost::filesystem::directory_iterator itr( inputDir );
itr != boost::filesystem::directory_iterator( );
++itr )
{
if ( !boost::filesystem::is_regular_file( itr->status( ) ) )
{
continue;
}
std::string filename = itr->path( ).filename( ).string( );
// check if prefix matches
if ( !prefix.empty( ) )
{
if ( filename.compare( 0, prefix.length( ), prefix ) != 0 )
{
continue;
}
}
// check if file extension matches
if ( filename.compare( filename.length( ) - fileExtension.length( ), fileExtension.length( ), fileExtension )
!= 0 )
{
continue;
}
imageFilenames.push_back( itr->path( ).string( ) );
if ( verbose )
{
std::cerr << "# INFO: Adding " << imageFilenames.back( ) << std::endl;
}
}
if ( imageFilenames.empty( ) )
{
std::cerr << "# ERROR: No chessboard images found." << std::endl;
return 1;
}
if ( verbose )
{
std::cerr << "# INFO: # images: " << imageFilenames.size( ) << std::endl;
}
std::sort( imageFilenames.begin( ), imageFilenames.end( ) );
cv::Mat image = cv::imread( imageFilenames.front( ), -1 );
const cv::Size frameSize = image.size( );
camodocal::CameraCalibration calibration( modelType, cameraName, frameSize, boardSize, squareSize );
calibration.setVerbose( verbose );
std::vector< bool > chessboardFound( imageFilenames.size( ), false );
for ( size_t i = 0; i < imageFilenames.size( ); ++i )
{
image = cv::imread( imageFilenames.at( i ), -1 );
camodocal::Chessboard chessboard( boardSize, image );
chessboard.findCorners( useOpenCV );
if ( chessboard.cornersFound( ) )
{
if ( verbose )
{
std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", "
<< imageFilenames.at( i ) << std::endl;
}
calibration.addChessboardData( chessboard.getCorners( ) );
cv::Mat sketch;
chessboard.getSketch( ).copyTo( sketch );
cv::imshow( "Image", sketch );
cv::waitKey( 50 );
}
else if ( verbose )
{
std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl;
}
chessboardFound.at( i ) = chessboard.cornersFound( );
}
cv::destroyWindow( "Image" );
if ( calibration.sampleCount( ) < 10 )
{
std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl;
return 1;
}
if ( verbose )
{
std::cerr << "# INFO: Calibrating..." << std::endl;
}
double startTime = camodocal::timeInSeconds( );
calibration.calibrate( );
calibration.writeParams( cameraName + "_camera_calib.yaml" );
calibration.writeChessboardData( cameraName + "_chessboard_data.dat" );
if ( verbose )
{
std::cout << "# INFO: Calibration took a total time of " << std::fixed
<< std::setprecision( 3 ) << camodocal::timeInSeconds( ) - startTime << " sec.\n";
}
if ( verbose )
{
std::cerr << "# INFO: Wrote calibration file to "
<< cameraName + "_camera_calib.yaml" << std::endl;
}
if ( viewResults )
{
std::vector< cv::Mat > cbImages;
std::vector< std::string > cbImageFilenames;
for ( size_t i = 0; i < imageFilenames.size( ); ++i )
{
if ( !chessboardFound.at( i ) )
{
continue;
}
cbImages.push_back( cv::imread( imageFilenames.at( i ), -1 ) );
cbImageFilenames.push_back( imageFilenames.at( i ) );
}
// visualize observed and reprojected points
calibration.drawResults( cbImages );
for ( size_t i = 0; i < cbImages.size( ); ++i )
{
cv::putText( cbImages.at( i ),
cbImageFilenames.at( i ),
cv::Point( 10, 20 ),
cv::FONT_HERSHEY_COMPLEX,
0.5,
cv::Scalar( 255, 255, 255 ),
1,
CV_AA );
cv::imshow( "Image", cbImages.at( i ) );
cv::waitKey( 0 );
}
}
return 0;
}

0
sensing/slam/vio/VINS-Mono/camera_model/src/sparse_graph/Transform.cc → sensing/slam/vio/VINS-Fusion/camera_models/src/sparse_graph/Transform.cc Executable file → Normal file
View File

64
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/a3_ptgrey_mono_imu_config.yaml Executable file
View File

@ -0,0 +1,64 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 1
imu_topic: "/djiros/imu"
image0_topic: "/stereo/left/image"
image1_topic: "/stereo/right/image"
output_path: "/home/dji/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 512
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.6001622181574202e-02, -5.2085872909730102e-03,
-9.9964833129574060e-01, -1.2886997952961168e-01,
9.9965481566775383e-01, 3.6294078574090616e-03,
-2.6020701583033656e-02, -1.2147595522352593e-01,
3.7636626038181076e-03, -9.9997984880548296e-01,
5.1124188763960188e-03, 3.2881561191272578e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

60
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/a3_ptgrey_stereo_config.yaml Executable file
View File

@ -0,0 +1,60 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 0
num_of_cam: 2
imu_topic: "/djiros/imu"
image0_topic: "/stereo/left/image"
image1_topic: "/stereo/right/image"
output_path: "/home/dji/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 512
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.6001622181574202e-02, -5.2085872909730102e-03,
-9.9964833129574060e-01, -1.2886997952961168e-01,
9.9965481566775383e-01, 3.6294078574090616e-03,
-2.6020701583033656e-02, -1.2147595522352593e-01,
3.7636626038181076e-03, -9.9997984880548296e-01,
5.1124188763960188e-03, 3.2881561191272578e-02, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.3877053002065107e-02, -1.2793564889566844e-02,
-9.9963303818819083e-01, -1.0696526695209820e-01,
9.9968048539671106e-01, 7.9915246532147322e-03,
-2.3980463938659879e-02, 3.8910400198456374e-03,
8.2953876903301693e-03, -9.9988622364303725e-01,
1.2598663101044294e-02, 3.1956268205111153e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

76
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/a3_ptgrey_stereo_imu_config.yaml Executable file
View File

@ -0,0 +1,76 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/djiros/imu"
image0_topic: "/pg_17533297/image"
image1_topic: "/pg_17593110/image"
#image0_topic: "/stereo/left/image"
#image1_topic: "/stereo/right/image"
output_path: "/home/tony-ws1/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 512
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -3.3711239846938357e-02, 5.5879405352924905e-03,
-9.9941599308223816e-01, -1.3488097324114276e-01,
9.9943103610356743e-01, 1.2644297085154799e-03,
-3.3704677575404185e-02, -1.1681178648179885e-01,
1.0753515387662560e-03, -9.9998358793436526e-01,
-5.6273866920459881e-03, 4.1641064011355096e-02, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.5349732839191397e-02, 8.6557147652394084e-04,
-9.9967826916013358e-01, -1.2784802722835098e-01,
9.9967221433053033e-01, 3.6084788347405805e-03,
-2.5346454899911497e-02, 2.2935031902104031e-03,
3.5853787074220378e-03, -9.9999311480955611e-01,
-9.5676172901448808e-04, 4.1243006399912775e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

77
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/a3_ptgrey_stereo_imu_config_backup.yaml Executable file
View File

@ -0,0 +1,77 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/djiros/imu"
image0_topic: "/pg_17533297/image"
image1_topic: "/pg_17593110/image"
#image0_topic: "/stereo/left/image"
#image1_topic: "/stereo/right/image"
output_path: "/home/dji/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 512
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.6001622181574202e-02, -5.2085872909730102e-03,
-9.9964833129574060e-01, -1.2886997952961168e-01,
9.9965481566775383e-01, 3.6294078574090616e-03,
-2.6020701583033656e-02, -1.2147595522352593e-01,
3.7636626038181076e-03, -9.9997984880548296e-01,
5.1124188763960188e-03, 3.2881561191272578e-02, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -2.3877053002065107e-02, -1.2793564889566844e-02,
-9.9963303818819083e-01, -1.0696526695209820e-01,
9.9968048539671106e-01, 7.9915246532147322e-03,
-2.3980463938659879e-02, 3.8910400198456374e-03,
8.2953876903301693e-03, -9.9988622364303725e-01,
1.2598663101044294e-02, 3.1956268205111153e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

16
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/left.yaml Executable file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 512
distortion_parameters:
k1: -1.3445051055717713e-01
k2: 9.3352440590059205e-02
p1: 1.9849609143174310e-05
p2: -2.2312866795141505e-04
projection_parameters:
fx: 4.4364768517023509e+02
fy: 4.4383960982739706e+02
cx: 3.1871281848422359e+02
cy: 2.4990737366327537e+02

16
sensing/slam/vio/VINS-Fusion/config/A3_ptgrey/right.yaml Executable file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 512
distortion_parameters:
k1: -1.3508083449623129e-01
k2: 9.2727834380078616e-02
p1: -2.0323824111007161e-06
p2: 2.6175094931305871e-04
projection_parameters:
fx: 4.4548064415136548e+02
fy: 4.4615298034311428e+02
cx: 3.1819340401904532e+02
cy: 2.5004333891088854e+02

18
sensing/slam/vio/VINS-Fusion/config/euroc/cam0_mei.yaml Normal file
View File

@ -0,0 +1,18 @@
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 752
image_height: 480
mirror_parameters:
xi: 3.6313355285286337e+00
distortion_parameters:
k1: 1.1757726639872075e+00
k2: 1.5491281051140213e+01
p1: -8.1237172954550494e-04
p2: 6.7297684030310243e-04
projection_parameters:
gamma1: 2.1387619122017772e+03
gamma2: 2.1315886210259278e+03
u0: 3.6119856633263799e+02
v0: 2.4827644773395667e+02

16
sensing/slam/vio/VINS-Fusion/config/euroc/cam0_pinhole.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.9545645106987750e-01
k2: 8.6623215640186171e-02
p1: 2.0132892276082517e-06
p2: 1.3924531371276508e-05
projection_parameters:
fx: 4.6115862106007575e+02
fy: 4.5975286598073296e+02
cx: 3.6265929181685937e+02
cy: 2.4852105668448124e+02

18
sensing/slam/vio/VINS-Fusion/config/euroc/cam1_mei.yaml Normal file
View File

@ -0,0 +1,18 @@
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 752
image_height: 480
mirror_parameters:
xi: 8.1261505894146113e-01
distortion_parameters:
k1: -3.4247886078397966e-01
k2: 1.7108792755973357e-01
p1: -6.6653144364906686e-04
p2: 7.2311255846767091e-04
projection_parameters:
gamma1: 8.3406249735437791e+02
gamma2: 8.3140606765916948e+02
u0: 3.7432007355249738e+02
v0: 2.5422391621480082e+02

16
sensing/slam/vio/VINS-Fusion/config/euroc/cam1_pinhole.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.9294124381930947e-01
k2: 8.4798002331543665e-02
p1: -2.9984646536002372e-04
p2: 3.0028216325237329e-04
projection_parameters:
fx: 4.6009781682258682e+02
fy: 4.5890983492218902e+02
cx: 3.7314916359808268e+02
cy: 2.5440734973672119e+02

62
sensing/slam/vio/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml Normal file
View File

@ -0,0 +1,62 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 1
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "~/output/"
cam0_calib: "cam0_mei.yaml"
cam1_calib: "cam1_mei.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation.
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
acc_w: 0.001 # accelerometer bias random work noise standard deviation.
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation.
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

55
sensing/slam/vio/VINS-Fusion/config/euroc/euroc_stereo_config.yaml Normal file
View File

@ -0,0 +1,55 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 0
num_of_cam: 2
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam0_mei.yaml"
cam1_calib: "cam1_mei.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

71
sensing/slam/vio/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml Normal file
View File

@ -0,0 +1,71 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "~/output/"
cam0_calib: "cam0_mei.yaml"
cam1_calib: "cam1_mei.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation.
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
acc_w: 0.001 # accelerometer bias random work noise standard deviation.
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation.
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

0
sensing/slam/vio/VINS-Mono/config/extrinsic_parameter_example.pdf → sensing/slam/vio/VINS-Fusion/config/extrinsic_parameter_example.pdf Executable file → Normal file
View File

0
sensing/slam/vio/VINS-Mono/config/fisheye_mask.jpg → sensing/slam/vio/VINS-Fusion/config/fisheye_mask.jpg
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 9.0 KiB

After

Width:  |  Height:  |  Size: 9.0 KiB

0
sensing/slam/vio/VINS-Mono/config/fisheye_mask_752x480.jpg → sensing/slam/vio/VINS-Fusion/config/fisheye_mask_752x480.jpg Executable file → Normal file
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 14 KiB

After

Width:  |  Height:  |  Size: 14 KiB

16
sensing/slam/vio/VINS-Fusion/config/gaas_sitl/cam0_pinhole_p1.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 376
fy: 376
cx: 376
cy: 240

16
sensing/slam/vio/VINS-Fusion/config/gaas_sitl/cam1_pinhole_p1.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 376
fy: 376
cx: 376
cy: 240

55
sensing/slam/vio/VINS-Fusion/config/gaas_sitl/euroc_stereo_config_plus5.yaml Normal file
View File

@ -0,0 +1,55 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 0
num_of_cam: 2
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam0_pinhole_p1.yaml" #"cam0_mei.yaml"
cam1_calib: "cam1_pinhole_p1.yaml" #"cam1_mei.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

73
sensing/slam/vio/VINS-Fusion/config/gaas_sitl/euroc_stereo_imu_config.yaml Normal file
View File

@ -0,0 +1,73 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/imu_stereo"
image0_topic: "/gi/forward/left/image_raw"
image1_topic: "/gi/forward/right/image_raw"
output_path: "/home/gi/output/"
cam0_calib: "cam0_pinhole_p1.yaml"
cam1_calib: "cam1_pinhole_p1.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0, 0, 1, 0,
-1, 0, 0, 0.12,
0, -1, 0, -0.3,
0, 0, 0, 1.]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0, 0, 1, 0,
-1, 0, 0, 0,
0, -1, 0, -0.3,
0, 0, 0, 1.]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 80 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.01 # accelerometer measurement noise standard deviation.
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
acc_w: 0.001 # accelerometer bias random work noise standard deviation.
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation.
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

16
sensing/slam/vio/VINS-Fusion/config/kitti_odom/cam00-02.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.188560000000e+02
fy: 7.188560000000e+02
cx: 6.071928000000e+02
cy: 1.852157000000e+02

16
sensing/slam/vio/VINS-Fusion/config/kitti_odom/cam03.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.215377000000e+02
fy: 7.215377000000e+02
cx: 6.095593000000e+02
cy: 1.728540000000e+02

16
sensing/slam/vio/VINS-Fusion/config/kitti_odom/cam04-12.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.070912000000e+02
fy: 7.070912000000e+02
cx: 6.018873000000e+02
cy: 1.831104000000e+02

16
sensing/slam/vio/VINS-Fusion/config/kitti_odom/cam13-21.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.188560000000e+02
fy: 7.188560000000e+02
cx: 6.071928000000e+02
cy: 1.852157000000e+02

65
sensing/slam/vio/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml Normal file
View File

@ -0,0 +1,65 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: "/leftImage"
image1_topic: "/rightImage"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam00-02.yaml"
cam1_calib: "cam00-02.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537165718864418,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

66
sensing/slam/vio/VINS-Fusion/config/kitti_odom/kitti_config03.yaml Normal file
View File

@ -0,0 +1,66 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: "/leftImage"
image1_topic: "/rightImage"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam03.yaml"
cam1_calib: "cam03.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537150588250621,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

65
sensing/slam/vio/VINS-Fusion/config/kitti_odom/kitti_config04-12.yaml Normal file
View File

@ -0,0 +1,65 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: "/leftImage"
image1_topic: "/rightImage"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam04-12.yaml"
cam1_calib: "cam04-12.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537150653267924,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

65
sensing/slam/vio/VINS-Fusion/config/kitti_odom/kitti_config13-21.yaml Normal file
View File

@ -0,0 +1,65 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: "/leftImage"
image1_topic: "/rightImage"
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam13-21.yaml"
cam1_calib: "cam13-21.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537165718864418,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

16
sensing/slam/vio/VINS-Fusion/config/kitti_raw/cam_09_30.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1226
image_height: 370
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.070912e+02
fy: 7.070912e+02
cx: 6.018873e+02
cy: 1.831104e+02

16
sensing/slam/vio/VINS-Fusion/config/kitti_raw/cam_10_03.yaml Normal file
View File

@ -0,0 +1,16 @@
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 7.188560000000e+02
fy: 7.188560000000e+02
cx: 6.071928000000e+02
cy: 1.852157000000e+02

65
sensing/slam/vio/VINS-Fusion/config/kitti_raw/kitti_09_30_config.yaml Normal file
View File

@ -0,0 +1,65 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: ""
image1_topic: ""
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam_09_30.yaml"
cam1_calib: "cam_09_30.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537150653267924,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

66
sensing/slam/vio/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml Normal file
View File

@ -0,0 +1,66 @@
%YAML:1.0
#common parameters
imu: 0
num_of_cam: 2
imu_topic: ""
image0_topic: ""
image1_topic: ""
output_path: "/home/tony-ws1/output/"
cam0_calib: "cam_10_03.yaml"
cam1_calib: "cam_10_03.yaml"
image_width: 1241
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [1, 0, 0, 0.537165718864418,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1]
#Multiple thread support
multiple_thread: 0
#feature traker paprameters
max_cnt: 200 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.08 # max solver itration time (s), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 1.0e-4 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

18
sensing/slam/vio/VINS-Fusion/config/mynteye/left_mei.yaml Normal file
View File

@ -0,0 +1,18 @@
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 752
image_height: 480
mirror_parameters:
xi: 1.4456544769803834e+00
distortion_parameters:
k1: -3.4363053357673362e-01
k2: 6.2347376791548004e-02
p1: 1.2608078788285675e-03
p2: -2.1374139252651483e-04
projection_parameters:
gamma1: 8.9594371861807986e+02
gamma2: 8.9741581479032754e+02
u0: 3.7221680816143993e+02
v0: 2.3833534175557006e+02

62
sensing/slam/vio/VINS-Fusion/config/mynteye/mynteye_mono_imu_config.yaml Normal file
View File

@ -0,0 +1,62 @@
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 1
imu_topic: "/mynteye/imu/data_raw"
image0_topic: "/mynteye/left/image_raw"
output_path: "/home/tong/output/"
cam0_calib: "left_mei.yaml"
image_width: 752
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 4.2812441490024389e-03, -9.9997001507473682e-01,
-6.4528985710044385e-03, 5.2583356071589790e-05,
9.9996900935734523e-01, 4.2384270612576547e-03,
6.6344601088757426e-03, -4.2174706544162562e-02,
-6.6069110351583190e-03, -6.4811023350536514e-03,
9.9995717110239080e-01, 1.9238715201769417e-02, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.8 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0

Some files were not shown because too many files have changed in this diff Show More