def __init__(self):
        rospy.init_node("graphoptimizer")

        self.posegraph = PoseGraph()
        self.rate = rospy.Rate(hz=RATE_HZ)

        self.pose_sub = rospy.Subscriber("pose",
                                         PoseStamped,
                                         self.on_pose,
                                         queue_size=1)
        self.scan_sub = rospy.Subscriber("scan",
                                         LaserScan,
                                         self.on_scan,
                                         queue_size=1)

        self.factorize_landmark_edges = rospy.get_param(
            "~factorize_landmark_edges", default=True)

        self.vertex_seq = WALL_VERTEX_ID  # landmark has id 1. The pose vertices will start with id of 2
        self.prev_pose = None
        self.last_msg_time = rospy.Time.now()

        self.has_wall_vertex = False
        self.prev_wallconstraint_vertexid = None
        self.landmark_key_vertices = []
        self.landmark_vertex_measurements = []
示例#2
0
 def __init__(self, raw_data, gui):
     self.gui = gui
     self.costmap = CostMap()
     x = base_x_
     y = base_y_
     yaw = base_yaw_
     self.scan_base = np.array(
         [[np.cos(base_yaw_), -np.sin(base_yaw_), base_x_],
          [np.sin(base_yaw_), np.cos(base_yaw_), base_y_], [0., 0., 1.]])
     self.pg = PoseGraph()  # LS-SLAM pose graph
     self.node_scan = self.readdata(raw_data)
     print("Generating graph...")
     self.creategraph(self.node_scan)
     print("Graph OK.")
     # Show graph before optimization
     self.show_graph()
     self.show_pointcloud()
示例#3
0
文件: demo.py 项目: 18968082/SLAM
import posegraph
import time
from utils import *
reload(posegraph)

from posegraph import PoseGraph
import matplotlib.pyplot as plt

# Specify files to load
vfile = 'data/killian-v.dat'
efile = 'data/killian-e.dat'

pg = PoseGraph()
pg.readGraph(vfile, efile)
"""
pg.readGraph('data/goalpoints_toro_result.graph')
cols = [(np.random.random(), np.random.random(), np.random.random()) for i in range(1000)]
coords = pg.nodes[:, :2]
uncertainties, path = get_uncertainties_and_path(coords)
for j in range(len(path)):
    plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
for e in [[20,8], [36,20], [98,36], [102,138], [112,128], [127,113]]:
    plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
    plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
    plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
    plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
plt.show()
"""

plt.ion()
plt.figure()
class PA3Optimizer:
    def __init__(self):
        rospy.init_node("graphoptimizer")

        self.posegraph = PoseGraph()
        self.rate = rospy.Rate(hz=RATE_HZ)

        self.pose_sub = rospy.Subscriber("pose",
                                         PoseStamped,
                                         self.on_pose,
                                         queue_size=1)
        self.scan_sub = rospy.Subscriber("scan",
                                         LaserScan,
                                         self.on_scan,
                                         queue_size=1)

        self.factorize_landmark_edges = rospy.get_param(
            "~factorize_landmark_edges", default=True)

        self.vertex_seq = WALL_VERTEX_ID  # landmark has id 1. The pose vertices will start with id of 2
        self.prev_pose = None
        self.last_msg_time = rospy.Time.now()

        self.has_wall_vertex = False
        self.prev_wallconstraint_vertexid = None
        self.landmark_key_vertices = []
        self.landmark_vertex_measurements = []

    def on_pose(self, posemsg):
        """Handles a pose message by adding a vertex (& odometry edge) to the graph optimizer
        
        Arguments:
            posemsg {PoseStamped} -- A newly recorded pose in the system
        """
        self.vertex_seq += 1
        self.posegraph.add_vertex(self.vertex_seq, posemsg.pose.position.x)
        rospy.loginfo("POSE #{0: 2d} @ {1: .3f}".format(
            self.vertex_seq, posemsg.pose.position.x))

        if self.prev_pose is not None:
            dx = posemsg.pose.position.x - self.prev_pose.position.x

            info_matrix = np.identity(3)
            info_matrix[0][0] = WHEEL_ODOMETRY_INFORMATION

            # Add an edge from the previous pose to the current one)
            self.posegraph.add_edge(fromVertex=self.vertex_seq - 1,
                                    toVertex=self.vertex_seq,
                                    dx=dx,
                                    information=info_matrix)
            rospy.loginfo("    => dx: {0:0.3f}".format(dx))

        # TODO: Add to the output file
        self.prev_pose = posemsg.pose
        self.last_msg_time = rospy.Time.now()

    def on_scan(self, scanmsg):
        """Handles a scan message by adding a landmark edge to the graph optimizer
        
        Arguments:
            scanmsg {LaserScan} -- A new scan reading
        """

        if self.prev_pose is None:
            rospy.logwarn("Ignoring scan b/c of lack of prev pose")
            # Can't provide an estimate to the wall position until we get our first pose measurement.
            return

        if self.prev_wallconstraint_vertexid is self.vertex_seq:
            rospy.logwarn(
                "Ignoring scan b/c we already encoded a constraint to the current vertex"
            )
            return

        # Note: for the robot, the first element in the laser scan message faces forward
        x_dist = scanmsg.ranges[0]

        if np.isinf(x_dist):
            rospy.logwarn("Ignoring scan b/c of infinite measurement")
            return

        x_std_dev = x_dist * SCAN_STDDEV_PERCENTAGE
        x_information = 1.0 / x_std_dev
        x_pos_estimate = self.prev_pose.position.x + x_dist

        if self.factorize_landmark_edges:
            self.addscan_factored(x_pos_estimate, x_information, x_dist)
        else:
            self.addscan_nonfactored(x_pos_estimate, x_information, x_dist)

    def addscan_nonfactored(self, x_pos_estimate, x_information, x_dist):

        if not self.has_wall_vertex:
            self.posegraph.add_landmark(WALL_VERTEX_ID, x_pos_estimate)
            self.has_wall_vertex = True
            rospy.loginfo(
                "Created landmark node at estimated position {0: .4f}".format(
                    x_pos_estimate))

        info_matrix = np.identity(2)
        info_matrix[0][0] = x_information

        self.posegraph.add_landmark_edge(   \
            vertex_id=self.vertex_seq,      \
            landmark_id=WALL_VERTEX_ID,     \
            dx=x_dist,                      \
            information=info_matrix)

        self.prev_wallconstraint_vertexid = self.vertex_seq

        self.last_msg_time = rospy.Time.now()
        rospy.loginfo(
            "    {0} to WALL: range {1: .4f}   est pos:{2: .4f}".format(
                self.vertex_seq, x_dist, x_pos_estimate))

    def addscan_factored(self, x_pos_estimate, x_information, x_dist):

        if self.prev_wallconstraint_vertexid is not None and \
           self.vertex_seq - self.prev_wallconstraint_vertexid < LANDMARK_FACTORIZATION_QUOTIENT:
            # This isn't a key vertex (one that will have landmark constraints added to it)
            return

        # Add an edge between current vertex and all existing key vertices
        for v, v_x_dist in zip(self.landmark_key_vertices,
                               self.landmark_vertex_measurements):

            dx = v_x_dist - x_dist

            info_matrix = np.identity(3)
            info_matrix[0][
                0] = x_information  # TODO: this should perhaps be an average of information value of current and previous scan

            # Add an edge from the previous pose to the current one)
            self.posegraph.add_edge(fromVertex=v,
                                    toVertex=self.vertex_seq,
                                    dx=dx,
                                    information=info_matrix)

        rospy.loginfo(
            "LANDMARK-FACTORIZED: #{0: 2d} landmark_x_dist:{1: .4f}".format(
                self.vertex_seq, x_dist))
        self.landmark_vertex_measurements.append(x_dist)
        self.landmark_key_vertices.append(self.vertex_seq)
        self.prev_wallconstraint_vertexid = self.vertex_seq

    def spin(self):

        # Ensure we have at least 1 vertex (e.g. that the rosbag has started playing),
        #   and then wait a bit after the last message before starting the optimization routine
        while not rospy.is_shutdown() and   \
              (self.vertex_seq == WALL_VERTEX_ID or rospy.Time.now() - self.last_msg_time < WAIT_TIME_BEFORE_OPTIMIZE):

            self.rate.sleep()

        rospy.loginfo("No new messages in {0}. Moving on".format(
            WAIT_TIME_BEFORE_OPTIMIZE))
        self.optimize()

    def optimize(self):
        self.pose_sub.unregister()
        self.scan_sub.unregister()

        preop_file = rospy.get_param("~preoptimization_output")
        rospy.loginfo(
            "Graph construction complete. Printing graph to {0}".format(
                preop_file))
        self.posegraph.save(preop_file)

        self.posegraph.optimize()

        postop_file = rospy.get_param("~postoptimization_output")
        rospy.loginfo(
            "Optimization complete. Printing graph to {0}".format(postop_file))
        self.posegraph.save(postop_file)
示例#5
0
class graphSLAM():
    def __init__(self, raw_data, gui):
        self.gui = gui
        self.costmap = CostMap()
        x = base_x_
        y = base_y_
        yaw = base_yaw_
        self.scan_base = np.array(
            [[np.cos(base_yaw_), -np.sin(base_yaw_), base_x_],
             [np.sin(base_yaw_), np.cos(base_yaw_), base_y_], [0., 0., 1.]])
        self.pg = PoseGraph()  # LS-SLAM pose graph
        self.node_scan = self.readdata(raw_data)
        print("Generating graph...")
        self.creategraph(self.node_scan)
        print("Graph OK.")
        # Show graph before optimization
        self.show_graph()
        self.show_pointcloud()
        #print("waiting create map ...")
        #self.show_map()

        #self.gui.setpcd(pos, adj)

    def show_graph(self):
        adj = np.array([[edge.id_from, edge.id_to] for edge in self.pg.edges])
        pos = self.pg.nodes[:, 0:2]
        self.gui.setgraph(pos, adj, self.loop_candidates)

    def show_pointcloud(self):
        pcd = np.array([[0, 0]])
        for scan, pose in self.node_scan:
            scan_trans = transpose_by_pose(pose, scan)
            pcd = np.vstack((pcd, scan_trans))
        self.gui.setpcd(pcd)

    def show_map(self, use_gaussian=False):

        self.costmap.map_data.fill(0)
        pcd = np.array([[0, 0]])
        for scan, pose in self.node_scan:
            scan_trans = transpose_by_pose(pose, scan)
            pcd = np.vstack((pcd, scan_trans))
        idx = self.costmap.world_map(pcd)

        for i in range(idx.shape[0]):
            p = idx[i, :].astype(int)
            if use_gaussian:
                self.costmap.updateCostMap(p, 30, True)
            else:
                self.costmap.updateCostMap(p, 10, False)
        self.gui.setmap(self.costmap.map_data)

    def update(self):
        pcd = np.array([[0, 0]])
        #self.node_scan.clear()
        node_scan = []
        #del self.node_scan[:]
        for i in range(len(self.node_scan)):
            scan, pose = self.node_scan[i]
            old_pose = self.pg.nodes[i, :]
            node_scan.append((scan, self.pg.nodes[i, :]))
        self.node_scan = node_scan

    def checkupdate(self, pre_pose, cur_pose):
        dx = pre_pose[0] - cur_pose[0]
        dy = pre_pose[1] - cur_pose[1]
        da = angle_diff(pre_pose[2], cur_pose[2])
        trans = sqrt(dx**2 + dy**2)
        update = (trans > d_thresh_) or (fabs(da) > a_thresh_)
        return update

    def optimize(self):
        print('Waiting optimization...')
        self.pg.optimize(opt_iter_)
        print('Optimization OK.')
        self.update()
        self.show_graph()
        self.show_pointcloud()

    def readdata(self, raw_data):
        data = []
        for scan, odom in raw_data:

            rot, trans = getRtFromM(self.scan_base)
            scan = transpose(rot, trans, scan)

            pose = t2v(odom).ravel()
            if len(data) == 0:
                data.append((scan, pose))

            if not self.checkupdate(data[-1][1], pose):
                continue

            data.append((scan, pose))

            #print pose
        return data

    def creategraph(self, data):
        nodes = []
        edges = []
        # Generate basic nodes and edges for our pose graph
        for i in range(len(data)):
            scan, pose = data[i]
            nodes.append(pose)
            self.pg.nodes.append(pose)
            if i == 0:
                continue
            infm = np.array([[weight_trans_, 0, 0], [0, weight_trans_, 0],
                             [0, 0, weight_rot_]])
            edge = [i - 1, i, get_motion_vector(nodes[i], nodes[i - 1]), infm]
            edges.append(edge)
            self.pg.edges.append(PoseEdge(*edge))
        self.pg.nodes = np.array(self.pg.nodes)

        # Detect the loop-closing
        loop_count = 0
        self.loop_candidates = []
        for i in range(len(data)):
            if i + min_loop_dist_ >= len(data):
                continue
            scan_i, pose_i = data[i]
            if not check_loop_candidate(scan_i, hough_weight_):
                continue
            self.loop_candidates.append(i)
            for j in range(i + min_loop_dist_, len(data)):
                scan_j, pose_j = data[j]
                dst = distance.euclidean(pose_i[0:2], pose_j[0:2])
                if dst > max_dist_:
                    continue
                #if not check_loop_candidate(scan_j, hough_weight_):
                #    continue
                loop_count += 1
                delta_odom_init = getDeltaM(v2t(pose_i), v2t(pose_j))
                delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)

                #start_time = time.time()
                icp = ICP(scan_i, scan_j)
                delta_rot, delta_trans, cost = icp.calculate(
                    200, delta_rot_init, delta_trans_init, max_dist_)
                if (cost > first_cost_):
                    #print 'old cost:',cost
                    delta_rot, delta_trans, cost = icp.calculate(
                        200, delta_rot, delta_trans, 0.5)
                    #print 'new cost:',cost
                    #print '-------------'
                if (use_cost_threshold_ and (cost > second_cost_)):
                    continue
                #cost = cost/5

                if icp_debug_:
                    new_scan_j = transpose(delta_rot, delta_trans.ravel(),
                                           scan_j)
                    scan_j = transpose(delta_rot_init,
                                       delta_trans_init.ravel(), scan_j)
                    show_icp_matching(
                        scan_i, new_scan_j, scan_j,
                        str(i) + '-->' + str(j) + 'cost:' + str(cost))
                    #plt.show()

                infm = np.array([[weight_trans_ / cost**2, 0, 0],
                                 [0, weight_trans_ / cost**2, 0],
                                 [0, 0, weight_rot_ / cost**2]])
                edge = [i, j, t2v(getMFromRt(delta_rot, delta_trans)), infm]
                edges.append(edge)
                self.pg.edges.append(PoseEdge(*edge))
        print('{0} loop have been detected!'.format(loop_count))
示例#6
0
import posegraph
import time
from utils import *
reload(posegraph)

from posegraph import PoseGraph
import matplotlib.pyplot as plt

# Specify files to load
vfile = 'data/killian-v.dat'
efile = 'data/killian-e.dat'

pg = PoseGraph()
pg.readGraph(vfile, efile)

"""
pg.readGraph('data/goalpoints_toro_result.graph')
cols = [(np.random.random(), np.random.random(), np.random.random()) for i in range(1000)]
coords = pg.nodes[:, :2]
uncertainties, path = get_uncertainties_and_path(coords)
for j in range(len(path)):
    plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
for e in [[20,8], [36,20], [98,36], [102,138], [112,128], [127,113]]:
    plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
    plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
    plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
    plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
plt.show()
"""

plt.ion()
示例#7
0
def runslam(coords, loopedges):
    uncertainties, path = get_uncertainties_and_path(coords, linear_uncertainty=ex, angular_uncertainty=ephi)
    
    pg = PoseGraph() # LS-SLAM pose graph
    edges = []
    # populate with data -  path
    for p in range(len(path)):
        pg.nodes.append(path[p])
        if p > 0:
            covm = uncertainties[p]#+uncertainties[p-1]
            try:
                infm=np.linalg.inv(covm)
            except:
                infm=np.linalg.inv(covm)
            edge = [p-1, p, get_motion_vector(path[p], path[p-1]), infm]
            edges.append(edge)
            pg.edges.append(PoseEdge(*edge))
    pg.nodes = np.array(pg.nodes)
    # populate with data - loop closures
    for idpair in loopedges:
        sx = minex
        sy = miney   
        st = minephi+ephi*np.sum([np.abs(path[p][2]-path[p-1][2]) for p in range(idpair[1], idpair[0]+1)])
        cov_prec = [
        [sx**2,0,0],
        [0,sy**2,0],
        [0,0,st**2]
        ]
        
        d = get_motion_vector([0,0]+[pg.nodes[idpair[0]][2]], [0,0]+[pg.nodes[idpair[1]][2]])
        print "inserting loop closure vector btw. ",idpair,": ", d, np.rad2deg(d[2]), "\t theta uncertainty:", cov_prec[2][2]
        try:
            edge = [idpair[0], idpair[1], d, np.linalg.inv(cov_prec)]
        except:
            edge = [idpair[0], idpair[1], d, np.linalg.inv(cov_prec)]
        edges.append(edge)
        pg.edges.append(PoseEdge(*edge))
        
    writePoseGraph(pg.nodes, edges, 'data/goalpoints.graph') # write graph in g2o format for TORO
        
    # run SLAM
    plt.clf()
    #plt.subplot(1,3,1)
    plt.title('path (before SLAM)')
    for j in range(len(path)):
        plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
        #plt.text(pg.nodes[j, 0]+30*float(j)/len(path)+np.random.random()*2-1, pg.nodes[j, 1]+np.random.random()*2-1, str(j))
    for e in loopedges:
        plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
        plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
        plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
        plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
    plt.draw()
    time.sleep(1)
    print "optimization step (1)"
    pg.optimize(1)
    
    for k in range(2):
        plt.subplot(1,2,k+1)
        plt.title('path (after '+str(k+1)+' SLAM steps)')
        plt.scatter(path[:, 0], path[:, 1], s=1)
        plt.hold(True)

        for j in range(len(path)):
            plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
            #plt.text(pg.nodes[j, 0], pg.nodes[j, 1], str(j))
        plt.draw()
        print "optimization step (5)"
        pg.optimize(5) 
        
    plt.show(block=True)
示例#8
0
文件: demo.py 项目: scomup/graphSLAM
base_x_ = 0.
base_y_ = 0.
base_yaw_ = 0.03
#base_yaw_ = 0.03
#base_yaw_ = -3.12
#base_x_ = -0.11
#base_y_ = 0.
#base_yaw_ = -1.54

##########################
d_thresh_ = .5
a_thresh_ = np.pi/6
##########################


pg = PoseGraph()

data = np.array([[0.,0,0],\
    [1,0,-0.57],\
    [1,-1,-1.14],\
    [0,-1,1.57],\
    [0,-0.2,0],\
    ])
loopedges = np.array([[0,4]])

def creategraph(data):
    nodes = []
    edges = []
    for i in range(len(data)):
        pose = data[i]
        nodes.append(pose)