Merge branch 'master' of https://gitee.com/robin_shaun/XTDrone
This commit is contained in:
commit
f9d19140ed
|
@ -3,5 +3,6 @@ competition
|
|||
*.bag
|
||||
*.pyc
|
||||
*.pbstream
|
||||
sensing/slam/vio/VINS-Fusion/camera_models/camera_calib_example
|
||||
.gitee
|
||||
.vscode
|
18
README.en.md
18
README.en.md
|
@ -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
20
README.md
|
@ -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" />
|
||||
|
||||
|
|
|
@ -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))
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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()
|
||||
|
||||
|
||||
|
|
|
@ -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]))
|
||||
|
|
|
@ -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:
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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' ' ')
|
||||
|
|
|
@ -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>
|
||||
|
|
Binary file not shown.
After Width: | Height: | Size: 901 KiB |
|
@ -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 -->
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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')
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.')
|
||||
|
||||
|
||||
|
|
@ -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')
|
||||
|
||||
|
||||
|
|
@ -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
0
sensing/slam/vio/VINS-Mono/LICENCE → sensing/slam/vio/VINS-Fusion/LICENCE
Executable file → Normal 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
15
sensing/slam/vio/VINS-Mono/camera_model/CMakeLists.txt → sensing/slam/vio/VINS-Fusion/camera_models/CMakeLists.txt
Executable file → Normal 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
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
|
@ -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,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
|
4
sensing/slam/vio/VINS-Mono/camera_model/package.xml → sensing/slam/vio/VINS-Fusion/camera_models/package.xml
Executable file → Normal file
4
sensing/slam/vio/VINS-Mono/camera_model/package.xml → sensing/slam/vio/VINS-Fusion/camera_models/package.xml
Executable file → Normal 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
0
sensing/slam/vio/VINS-Mono/camera_model/readme.md → sensing/slam/vio/VINS-Fusion/camera_models/readme.md
Executable file → Normal 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( );
|
||||
}
|
||||
}
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
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
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
|
@ -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,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
|
|
@ -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)
|
||||
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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)
|
|
@ -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
|
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
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
Before Width: | Height: | Size: 14 KiB After Width: | Height: | Size: 14 KiB |
|
@ -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
|
|
@ -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
|
|
@ -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)
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
|
@ -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
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
@ -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
|
|
@ -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
Loading…
Reference in New Issue