Esempio n. 1
0
def plot_data(in_data: Classes.InData, out_data: Classes.OutData):
    pos_ned = in_data.gnss
    x_h = out_data.x_h
    [lat_ref, lon_ref] = [[el[0] for el in in_data.gnssref],
                          [el[1] for el in in_data.gnssref]]
    [lat_ref,
     lon_ref] = [sum(lat_ref) / len(lat_ref),
                 sum(lon_ref) / len(lon_ref)]
    x_ned = nv.ned2lla(
        [[el[0][0] for el in x_h], [el[1][0]
                                    for el in x_h], [el[2][0] for el in x_h]],
        lat_ref, lon_ref, 0)  # convert ned to lat lon for output to html
    x_ned_list = []
    for i in range(len(x_ned[0])):
        x_ned_list.append([x_ned[2][i], x_ned[0][i], x_ned[1][i]
                           ])  # append the lat long for the output format
    with open('../Output/output.csv', 'wt',
              newline='') as file:  # write the data as lat long to output file
        filewc = csv.writer(file, dialect='excel')
        filewc.writerows(x_ned_list)
    plt.figure(1)
    aided = plt.plot(
        [el[1] for el in x_h], [el[0] for el in x_h],
        'r-.',
        label='GNSS aided INS trajectory')  # plot the GPS aided INS trajectory

    x_ned_1 = nv.ned2lla(
        [[el[0][0] for el in x_h], [el[1][0]
                                    for el in x_h], [el[2][0] for el in x_h]],
        lat_ref, lon_ref, 0)
    x_ned_list_limited = []
    for i in range(len(x_ned_1[0])):
        x_ned_list_limited.append(
            [x_ned_1[2][i], x_ned_1[0][i], x_ned_1[1][i]])
    # --------for file with GPS signal------------------------------------
    # for files with no GPS signals comment this section
    GPS = plt.plot([el[1] for el in pos_ned], [el[0] for el in pos_ned],
                   'b--',
                   label='GNSS position estimate')  # plot the GPS signal
    start = plt.plot(pos_ned[0][1],
                     pos_ned[0][0],
                     'ks',
                     linewidth=4.0,
                     label='Start point')  # plot the start point
    #  ------------------------------------------
    plt.title('Trajectory')
    plt.ylabel('North [m]')
    plt.xlabel('East [m]')
    plt.legend()
    plt.grid(True)
    plt.axis('equal')
    plt.show()
Esempio n. 2
0
    def get_corners(self, lon_lat_h, C_n_v):
        """
        Returns the corners of the image in WGS Coordinates and NED
        """
        # Find point on terrain directly below lon_lat_h
        ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
        ref_lla = ref_lla.flatten()
        c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
                              ref_lla[1], ref_lla[0], ref_lla[2])
        R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
        n = np.array([0.0, 0.0, 1.0])
        K = self.K
        Ki = np.linalg.inv(K)
        corners_pix = np.hstack((self.corners_pix, np.ones((4, 1)))).T
        cvec = np.dot(Ki, corners_pix)
        pclos = cvec / np.linalg.norm(cvec, axis=0)
        pwlos = np.dot(R_w_c, pclos)
        dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
        corners_local = ((-1 * dd * pwlos) + c_w_0.reshape(3, 1)).T

        # Return these for KML Style (clockwise, starting with Lower Left)
        corners_local = corners_local[[1, 2, 3, 0], :]
        out_pix = (corners_pix.T)[[1, 2, 3, 0], :]
        c_wgs = np.vstack(navpy.ned2lla(corners_local,
                                        ref_lla[1], ref_lla[0], ref_lla[2])).T
        return corners_local, c_wgs[:, [1, 0, 2]], out_pix
Esempio n. 3
0
def features_from_slice(ophoto,
                        slc,
                        detector,
                        desc_extract,
                        terrain_handler,
                        zoom=15,
                        bias=None):
    """
    Takes in an orthophoto, slice tuple, detector, and extractor
    """
    sub_photo = ophoto.get_img_from_slice(slc)
    kp1, desc1 = f2d.extract_features(sub_photo.img, detector, desc_extract)
    if len(kp1) > 0:
        pix = f2d.keypoint_pixels(kp1)
        meta = np.array([(kp.angle, kp.response, kp.size) for kp in kp1])
        packed = f2d.numpySIFTScale(kp1)
        pts_wgs84 = sub_photo.pix_to_wgs84(pix)
        pts_hae = terrain_handler.add_heights(pts_wgs84)
        if bias is not None:
            print(bias)
            ref = pts_hae[0, [1, 0, 2]]
            pts_ned = navpy.lla2ned(pts_hae[:, 1], pts_hae[:, 0],
                                    pts_hae[:, 2], *ref)
            pts_ned -= bias
            pts_hae = np.vstack(navpy.ned2lla(pts_ned, *ref)).T
            pts_hae = pts_hae[:, [1, 0, 2]]

        tile_coords = np.array(
            [np.array(mercantile.tile(pt[0], pt[1], zoom)) for pt in pts_hae])
        return (np.hstack((tile_coords, pts_hae, packed, meta)), desc1)
    else:
        return (None, None)
Esempio n. 4
0
    def update_rover_virtual_PosVelEcef(self, dt):

        #calculate virtual position in ecef frame with noise
        self.rover_ned_noise = self.add_gps_noise(self.white_noise_3d,
                                                  self.rover_ned_noise,
                                                  self.sigma_rover_pos)
        rover_ned_w_noise = self.rover_ned + self.rover_ned_noise
        self.rover_virtual_lla = navpy.ned2lla(rover_ned_w_noise,
                                               self.ref_lla[0],
                                               self.ref_lla[1],
                                               self.ref_lla[2])
        self.rover_virtual_pos_ecef = navpy.lla2ecef(self.rover_virtual_lla[0],
                                                     self.rover_virtual_lla[1],
                                                     self.rover_virtual_lla[2])

        #calculate virtual velocity in ecef frame with noise
        #make sure we do not divide by zero
        if dt != 0.0:
            rover_vel = (self.rover_ned - self.rover_ned_prev) / dt
        else:
            rover_vel = np.zeros(3)
        self.rover_vel_noise = self.add_noise_3d(np.zeros(3),
                                                 self.white_noise_3d)
        self.rover_vel_lpf = self.lpf(self.rover_vel_noise,
                                      self.rover_vel_noise_prev, self.Ts,
                                      self.sigma_rover_vel)
        rover_vel_w_noise = rover_vel + self.rover_vel_lpf
        self.rover_virtual_vel_ecef = navpy.ned2ecef(
            rover_vel_w_noise, self.ref_lla[0], self.ref_lla[1],
            self.ref_lla[2])  #self.ned2ecef(rover_vel_w_noise, self.ref_lla)

        #update histories
        self.rover_ned_prev = self.rover_ned
        self.rover_vel_prev = rover_vel
        self.rover_vel_noise_prev = self.rover_vel_noise
Esempio n. 5
0
def N_to_NED(data, static_x, lat, long, h):
    cn.it += 1
    static_NED = pd.DataFrame(index=data.index,
                              columns=['x', 'y', 'z', 'lat', 'long', 'alt'])
    for i in range(len(static_x)):
        if cn.it != 4:
            ned = [
                float(static_x[i][0]),
                float(static_x[i][1]),
                float(static_x[i][2])
            ]
        else:
            ned = [
                float(static_x.iloc[i, 0]),
                float(static_x.iloc[i, 1]),
                float(static_x.iloc[i, 2])
            ]
        static_NED.iloc[i, 0] = ned[0]
        static_NED.iloc[i, 1] = ned[1]
        static_NED.iloc[i, 2] = ned[2]
        coord = navpy.ned2lla(ned,
                              lat * 180 / m.pi,
                              long * 180 / m.pi,
                              h,
                              latlon_unit='deg',
                              alt_unit='m',
                              model='wgs84')
        static_NED.iloc[i, 3] = coord[0]
        static_NED.iloc[i, 4] = coord[1]
        static_NED.iloc[i, 5] = coord[2]
    return static_NED
Esempio n. 6
0
def load(path, extern_ref):
    result = []

    # load matches
    matches = pickle.load(open(path, "rb"))
    print("loaded features:", len(matches))

    # load project file to get ned reference for match coordinates
    dir = os.path.dirname(path)
    project_file = dir + "/Project.json"
    try:
        f = open(project_file, 'r')
        project_dict = json.load(f)
        f.close()
        ref = project_dict['ned-reference-lla']
        print('features ned reference:', ref)
    except:
        print("error: cannot load", project_file)

    # convert match coords to lla, then back to external ned reference
    # so the calling layer can have the points in the calling
    # reference coordinate system.
    print("converting feature coordinates to movie ned coordinate frame")
    for m in matches:
        zombie_door = random.randint(0, 49)
        if zombie_door == 0:
            ned = m[0]
            if ned != None:
                lla = navpy.ned2lla(ned, ref[0], ref[1], ref[2])
                newned = navpy.lla2ned(lla[0], lla[1], lla[2], extern_ref[0],
                                       extern_ref[1], extern_ref[2])
                result.append(newned)
    return result
Esempio n. 7
0
    def find_center_point(self, lon_lat_h, C_n_v):
        """
        Get the world points of the 4 corners of the image and return them
        :param np.ndarray lon_lat_h: Lon, Lat, Height in (3,) in degrees
        :param np.ndarray att: (3,3) of DCM representing C_n_v
        """
        # Find point on terrain directly below lon_lat_h
        ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
        ref_lla = ref_lla.flatten()

        c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
                              ref_lla[1], ref_lla[0], ref_lla[2])
        R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
        n = np.array([0.0, 0.0, 1.0])
        K = self.K
        Ki = np.linalg.inv(K)
        center_pix = np.array([self.cam_width / 2.0,
                               self.cam_height / 2.0,
                               1.0])
        cvec = np.dot(Ki, center_pix)
        pclos = cvec / np.linalg.norm(cvec, axis=0)
        pwlos = np.dot(R_w_c, pclos)
        dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
        center_local = ((-1 * dd * pwlos) + c_w_0)

        # Return these for KML Style (clockwise, starting with Lower Left)
        c_wgs = navpy.ned2lla(center_local, ref_lla[1], ref_lla[0], ref_lla[2])
        c_wgs = np.array(c_wgs)
        return c_wgs[[1, 0, 2]]
    def __init__(self, Var1, Var2, Var3, type, referencePoint):
        Var1 = float(Var1)
        Var2 = float(Var2)
        Var3 = float(Var3)
        if type == 1:
            self.lat = Var1
            self.long = Var2
            self.alt = Var3

            nedArray = lla2ned(Var1, Var2, Var3, referencePoint[0],
                               referencePoint[1], referencePoint[2])
            self.north = nedArray[0]
            self.east = nedArray[1]
            self.down = nedArray[2]

        elif type == 2:
            self.north = Var1
            self.east = Var2
            self.down = Var3

            llaArray = ned2lla(numpy.array([Var1, Var2,
                                            Var3]), referencePoint[0],
                               referencePoint[1], referencePoint[2])
            self.lat = llaArray[0]
            self.long = llaArray[1]
            self.alt = llaArray[2]
Esempio n. 9
0
 def load_tiles(self, lla_ref, width_m, height_m):
     print "Notice: loading DEM tiles"
     ll_ned = np.array([[-height_m*0.5, -width_m*0.5, 0.0]])
     ur_ned = np.array([[height_m*0.5, width_m*0.5, 0.0]])
     ll_lla = navpy.ned2lla(ll_ned, lla_ref[0], lla_ref[1], lla_ref[2])
     ur_lla = navpy.ned2lla(ur_ned, lla_ref[0], lla_ref[1], lla_ref[2])
     #print ll_lla
     #print ur_lla
     lat1, lon1 = lla_ll_corner( ll_lla[0], ll_lla[1] )
     lat2, lon2 = lla_ll_corner( ur_lla[0], ur_lla[1] )
     for lat in range(lat1, lat2+1):
         for lon in range(lon1, lon2+1):
             srtm = SRTM(lat, lon, '../srtm')
             if srtm.parse():
                 srtm.make_lla_interpolator()
                 #srtm.plot_raw()
                 tile_name = make_tile_name(lat, lon)
                 self.tile_dict[tile_name] = srtm
Esempio n. 10
0
 def load_tiles(self, lla_ref, width_m, height_m):
     print("Notice: loading DEM tiles")
     ll_ned = np.array([[-height_m * 0.5, -width_m * 0.5, 0.0]])
     ur_ned = np.array([[height_m * 0.5, width_m * 0.5, 0.0]])
     ll_lla = navpy.ned2lla(ll_ned, lla_ref[0], lla_ref[1], lla_ref[2])
     ur_lla = navpy.ned2lla(ur_ned, lla_ref[0], lla_ref[1], lla_ref[2])
     #print ll_lla
     #print ur_lla
     lat1, lon1 = lla_ll_corner(ll_lla[0], ll_lla[1])
     lat2, lon2 = lla_ll_corner(ur_lla[0], ur_lla[1])
     for lat in range(lat1, lat2 + 1):
         for lon in range(lon1, lon2 + 1):
             srtm = SRTM(lat, lon, '../srtm')
             if srtm.parse():
                 srtm.make_lla_interpolator()
                 #srtm.plot_raw()
                 tile_name = make_tile_name(lat, lon)
                 self.tile_dict[tile_name] = srtm
Esempio n. 11
0
    def features_from_slice(self,
                            slc,
                            bias=None,
                            convert_to_gray=True,
                            color_code=cv2.COLOR_RGBA2GRAY):
        """
        Takes in an orthophoto, slice tuple, detector, and extractor
        """
        sub_photo = self._ophoto.get_img_from_slice(slc)

        if (sub_photo.img.ndim > 2) and convert_to_gray:
            sub_img = cv2.cvtColor(sub_photo.img, color_code)
        elif (sub_photo.img.ndim > 2):
            raise TypeError(
                "Underlying image is greater than 2 dimensions and convert_to_gray is set to false"
            )
        else:
            sub_img = sub_photo.img

        kp, desc = f2d.extract_features(sub_img, self._detector,
                                        self._desc_extractor)
        if len(kp) > 0:
            pix = f2d.keypoint_pixels(kp)
            meta = np.array([(pt.angle, pt.response, pt.size) for pt in kp])
            packed = f2d.numpySIFTScale(kp)

            img_df = pd.DataFrame(np.hstack((pix, meta, packed)),
                                  columns=[
                                      'pix_x', 'pix_y', 'angle', 'response',
                                      'size', 'octave', 'layer', 'scale'
                                  ])

            pts_lon_lat = sub_photo.pix_to_wgs84(pix)
            pts_wgs = self._terrain_handler.add_heights(pts_lon_lat)
            if bias is not None:
                print(bias)
                ref = pts_wgs[0, [1, 0, 2]]
                pts_ned = navpy.lla2ned(pts_wgs[:, 1], pts_wgs[:, 0],
                                        pts_wgs[:, 2], *ref)
                pts_ned -= bias
                pts_wgs = np.vstack(navpy.ned2lla(pts_ned, *ref)).T
                pts_wgs = pts_wgs[:, [1, 0, 2]]

            df = pd.concat([
                img_df,
                pd.DataFrame(pts_wgs, columns=['lon', 'lat', 'height'])
            ],
                           axis=1)

            # Sort by feature response
            df.sort_values('response', ascending=False, inplace=True)
            pp = np.copy(df.index)
            df.index = np.arange(df.shape[0])
            return df, desc[pp, :]
        else:
            return None, None
def compute_rover_gps(roverTruth, gps, latRef, lonRef, altRef, originEcef):
    gps.lla = navpy.ned2lla(roverTruth.position, latRef, lonRef, altRef)

    ecefPositionRelative = navpy.ned2ecef(roverTruth.position, latRef, lonRef,
                                          altRef)
    gps.positionEcef = ecefPositionRelative + originEcef

    gps.velocityEcef = navpy.ned2ecef(roverTruth.velocity, latRef, lonRef,
                                      altRef)

    gps.fix = 3
Esempio n. 13
0
async def run_mission(drone, mission_items, lla_ref, gz_sub):
    end_point = [38, 0, 1]

    # The ned values are slightly skewed because the lla reference is at the start location, not world origin
    # lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
    lla_ref_off = [0, 0, 0]

    [lat, lon, alt] = lla_ref  # Set default mission item to start location

    print("-- Comencing mission")
    while (1):
        lsr_val = await gz_sub.get_LaserScanStamped()

        # Get next wp and determine if done
        print("-- Getting next wp")
        [x, y, z, speed] = get_next_wp(lsr_val, lla_ref_off)
        if (not np.isnan(x)):
            [lat, lon, alt] = navpy.ned2lla([y, x, -z], lla_ref[0], lla_ref[1],
                                            lla_ref[2])
            await drone.action.set_maximum_speed(speed)
            await drone.action.goto_location(lat, lon, alt, 90)
        else:
            own_x = lsr_val.scan.world_pose.position.x + lla_ref_off[1]
            own_z = lsr_val.scan.world_pose.position.z - lla_ref_off[2]
            print("-- Going to the end")
            # print("-- Mission is done")
            await drone.action.set_maximum_speed(1)
            [lat, lon,
             alt] = navpy.ned2lla([end_point[1], end_point[0], -end_point[2]],
                                  lla_ref[0], lla_ref[1], lla_ref[2])
            if (own_x >= end_point[0] - 5
                    and abs(own_z - end_point[2]) <= 0.5):
                print("-- landing")
                await drone.action.land()
                break
            else:
                await drone.action.goto_location(lat, lon, alt, 90)
Esempio n. 14
0
def calc_least_squares(data, H, rho, lat, long, h):
    rover_pos = pd.DataFrame(index=data.index, columns=['x','y','z','lat','long','alt', 'p_inv'])
    for i in range(848):
        p_inv = la.inv(H[i].T @ H[i])
        x_hat = p_inv @ H[i].T @ rho[i]
        NED = [float(x_hat[0]), float(x_hat[1]), float(x_hat[2])]
        rover_pos.iloc[i, 0] = float(x_hat[0])
        rover_pos.iloc[i, 1] = float(x_hat[1])
        rover_pos.iloc[i, 2] = float(x_hat[2])
        coord = navpy.ned2lla(NED, lat * 180 / m.pi, long * 180 / m.pi, h, latlon_unit='deg', alt_unit='m',
                                 model='wgs84')
        rover_pos.iloc[i, 3] = coord[0]
        rover_pos.iloc[i, 4] = coord[1]
        rover_pos.iloc[i, 5] = coord[2]
        rover_pos.iloc[i, 6] = p_inv
    return rover_pos
Esempio n. 15
0
    def project_points(self, lon_lat_h, C_n_v, points, return_local=False):
        """
        This is the key Function to project points from the image frame into
        the Local Level (NED) frame, and then convert into WGS-84. The origin
        of the local level (NED) frame is located as the (Lon, Lat) of the
        aircraft, with the height being located at h = Dem(lon, lat) (eg,
        terrain height). This function computes the points by solving for
        the ray-local level plane intersection
        :param lon_lat_h: [3,] Position of the camera in Lon, Lat (deg),
            height (m)
        :param C_n_v: [3x3] np.ndarray that translates a vector in the vehicle
            frame into the local-level NED frame
        :param points: [Nx2] [np.ndarray] of points to project
        :return: Returns an [Nx3] np.ndarray of Lon, Lat, Height per point in
            [points]
        """
        if self.K is None:
            raise ValueError("Camera Model Not Initialized")

        if points.ndim == 1:
            points = points.reshape(1, 2)

        ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
        ref_lla = ref_lla.flatten()
        c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
                              ref_lla[1], ref_lla[0], ref_lla[2])
        R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
        n = np.array([0.0, 0.0, 1.0])
        K = self.K
        Ki = np.linalg.inv(K)

        pix = np.hstack((points, np.ones((points.shape[0], 1)))).T
        cvec = np.dot(Ki, pix)
        pclos = cvec / np.linalg.norm(cvec, axis=0)
        pwlos = np.dot(R_w_c, pclos)
        dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
        points_local = ((-1 * dd * pwlos) + c_w_0.reshape(3, 1)).T

        c_wgs = np.vstack(
            navpy.ned2lla(points_local, ref_lla[1], ref_lla[0], ref_lla[2])).T
        if return_local:
            return c_wgs[:, [1, 0, 2]], points_local
        else:
            return c_wgs[:, [1, 0, 2]]
Esempio n. 16
0
    def save(self):
        filename = os.path.join(self.project_dir, 'annotations.json')
        print('Saving annotations:', filename)
        lla_list = []
        for m in self.markers:
            lla = navpy.ned2lla( [m[1], m[0], 0.0],
                                 self.ned_ref[0],
                                 self.ned_ref[1],
                                 self.ned_ref[2] )
            lla_list.append(lla)
        f = open(filename, 'w')
        json.dump(lla_list, f, indent=4)
        f.close()

        # write simple csv version
        filename = os.path.join(self.project_dir, 'annotations.csv')
        with open(filename, 'w') as f:
            fieldnames = ['lat_deg', 'lon_deg']
            writer = csv.DictWriter(f, fieldnames=fieldnames)
            writer.writeheader()
            for lla in lla_list:
                writer.writerow({'lat_deg': lla[0], 'lon_deg': lla[1]})
Esempio n. 17
0
    def test_ned2lla(self):
        """
        Test conversion from NED to LLA.

        Data Source: derived from "test_lla2ned" above.
        """
        # A point near Los Angeles, CA
        lat_ref = +( 34. + 0./60 + 0.00174/3600)  # North
        lon_ref = -(117. + 20./60 + 0.84965/3600)  # West
        alt_ref = 251.702 # [meters]  

        # Point near by with known NED position
        lat = +(34.  + 0./60 + 0.19237/3600) # North
        lon = -(117. +20./60 + 0.77188/3600) # West
        alt =  234.052 # [meters]

        ned = [5.8738, 1.9959, 17.6498]

        # Do conversion and check result
        # Note: default assumption on units is deg and meters
        lla_computed = navpy.ned2lla(ned, lat_ref, lon_ref, alt_ref)
        
        for e1, e2 in zip(lla_computed, [lat, lon, alt]):
            self.assertAlmostEqual(e1, e2, places=3)        
Esempio n. 18
0
    def test_ned2lla(self):
        """
        Test conversion from NED to LLA.

        Data Source: derived from "test_lla2ned" above.
        """
        # A point near Los Angeles, CA
        lat_ref = +(34. + 0. / 60 + 0.00174 / 3600)  # North
        lon_ref = -(117. + 20. / 60 + 0.84965 / 3600)  # West
        alt_ref = 251.702  # [meters]

        # Point near by with known NED position
        lat = +(34. + 0. / 60 + 0.19237 / 3600)  # North
        lon = -(117. + 20. / 60 + 0.77188 / 3600)  # West
        alt = 234.052  # [meters]

        ned = [5.8738, 1.9959, 17.6498]

        # Do conversion and check result
        # Note: default assumption on units is deg and meters
        lla_computed = navpy.ned2lla(ned, lat_ref, lon_ref, alt_ref)

        for e1, e2 in zip(lla_computed, [lat, lon, alt]):
            self.assertAlmostEqual(e1, e2, places=3)
Esempio n. 19
0
def main():

    # TEST
    # u = 1
    # v = 2
    # uv = np.array([u, v, 1])
    # y = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
    # print(np.dot(y, uv))
    # print(y)

    A = getCameraIntrinsicMatrix()
    # print(A)

    #test = np.array([[4, 7], [2, 6]])
    #print(np.linalg.inv(test))

    # Get camera's GPS positions and height
    lat, lon, alt = getCameraPosition()
    zc = getCameraHeight()

    # Transform camera position from Geodetic (lla) to NED
    # A point near Los Angeles, CA, given in https://github.com/NavPy/NavPy/blob/master/navpy/core/tests/test_navpy.py
    # (this will change to the location of the experiment later on)
    lat_ref = +(34. + 0./60 + 0.00174/3600) # North
    lon_ref = -(117. + 20./60 + 0.84965/3600) # West
    alt_ref = 251.702 # in meters
    rnnc = navpy.lla2ned(lat, lon, alt, lat_ref, lon_ref, alt_ref, latlon_unit='deg', alt_unit='m', model='wgs84')
    #print(rnnc)
    # TEST
    #lat_ref = 34.004924
    #lon_ref = -117.897842
    #alt_ref = 129.9972
    rnnc = np.array([0, 0, -zc])
    #rnnc = np.array([0, 0, -1.3462])


    # Get the body frame’s orientation angles and gimbal angles from the GPS
    roll, pitch, yaw = getCameraOrientation()
    tilt, pan = getGimbalAngles()
    # print(roll, pitch, yaw, tilt, pan)

    # Compute Rcn from roll, pitch, yaw, tilt, pan angles
    Rnb = np.array([[m.cos(yaw)*m.cos(pitch), -m.sin(yaw)*m.cos(roll) + m.cos(yaw)*m.sin(roll)*m.sin(pitch), m.sin(yaw)*m.sin(roll) + m.cos(yaw)*m.cos(roll)*m.sin(pitch)],
                    [m.sin(yaw)*m.cos(pitch), m.cos(yaw)*m.cos(roll) + m.sin(roll)*m.sin(pitch)*m.sin(yaw), -m.cos(yaw)*m.sin(roll) + m.sin(pitch)*m.sin(yaw)*m.cos(roll)],
                    [-m.sin(pitch), m.cos(pitch)*m.sin(roll), m.cos(pitch)*m.cos(roll)]])
    Rcm = np.array([[0, 1, 0],
                    [-1, 0, 0],
                    [0, 0, 1]])
    Rmb = np.array([[m.cos(pan)*m.cos(tilt), m.sin(pan)*m.cos(tilt), -m.sin(tilt)],
                    [-m.sin(pan), m.cos(pan), 0],
                    [m.cos(pan)*m.sin(tilt), m.sin(pan)*m.sin(tilt), m.cos(tilt)]])
    Rmb = np.array([[1, 0, 0],
                    [0, 1, 0],
                    [0, 0, 1]])
    # Rmb is simply the identity matrix if camera is strapped directly to airframe
    Rcn = np.linalg.inv(np.dot(Rnb, np.linalg.inv(np.dot(Rcm, Rmb))))
    #print(Rcn)

    # Compute G(NE) from Rcn and rnnc
    Gne = Rcn.copy()
    Gne[:, 2] = -np.dot(Rcn, rnnc)
    #print(Gne)

    # Obtain the pixel positions (u,v) of hot spots from fire detection process
    image = Image.open("location1_fire.jpg")
    pixels = image.load()
    width = image.size[0]
    height = image.size[1]
    # print(width, height)
    for i in range(0, height):
        for j in range(0, width):
            if (pixels[j, i] != 0):
                uv = np.array([j, i, 1])

                # Testing the pinhole camera model
                #xyzc = zc * np.dot(np.linalg.inv(A), uv)
                #print(m.sqrt(xyzc[0] * xyzc[0] + xyzc[1] * xyzc[1] + xyzc[2] * xyzc[2]))

                # Compute the NE coordinates of the hot spots from G(NE)
                NE = zc * np.dot(np.dot(np.linalg.inv(Gne), np.linalg.inv(A)), uv)
                #print(m.sqrt(NE[0] * NE[0] + NE[1] * NE[1]))

                # Converts hot spots' coordinates from NED to lla (GPS) coordinates
                result = navpy.ned2lla([NE[0], NE[1], 0], lat_ref, lon_ref, alt_ref, latlon_unit='deg', alt_unit='m', model='wgs84')
                print(result)
Esempio n. 20
0
camw, camh = proj.cam.get_image_params()
for image in proj.image_list:
    print image.name
    scale = float(image.width) / float(camw)
    K = proj.cam.get_K(scale)
    IK = np.linalg.inv(K)
    corner_list = []
    corner_list.append([0, image.height])
    corner_list.append([image.width, image.height])
    corner_list.append([image.width, 0])
    corner_list.append([0, 0])

    proj_list = proj.projectVectors(IK, image, corner_list)
    print "proj_list:\n", proj_list
    # pts = proj.intersectVectorsWithGroundPlane(image.camera_pose,
    #                                           g, proj_list)
    pts = sss.interpolate_vectors(image.camera_pose, proj_list)
    # print "pts (ned):\n", pts

    corners_lonlat = []
    for ned in pts:
        print ned
        lla = navpy.ned2lla([ned], ref[0], ref[1], ref[2])
        corners_lonlat.append([lla[1], lla[0]])
    ground = kml.newgroundoverlay(name=image.name)
    ground.icon.href = "Images/" + image.name
    ground.gxlatlonquad.coords.addcoordinates(corners_lonlat)

filename = args.project + "/GroundOverlay.kml"
kml.save(filename)
Esempio n. 21
0
    
    proj_list = proj.projectVectors( IK, image, corner_list, pose=args.pose )
    #print "proj_list:\n", proj_list
    if args.pose == 'direct':
        pts_ned = sss.interpolate_vectors(image.camera_pose, proj_list)
    elif args.pose == 'sba':
        pts_ned = sss.interpolate_vectors(image.camera_pose_sba, proj_list)
    # print "pts (ned):\n", pts_ned
    
    image.corner_list_ned = []
    image.corner_list_lla = []
    image.corner_list_xy = []
    for ned in pts_ned:
        #print p
        image.corner_list_ned.append( [ned[0], ned[1]] )
        image.corner_list_lla.append( navpy.ned2lla([ned], ref[0], ref[1], ref[2]) )
        image.corner_list_xy.append( [ned[1], ned[0]] )


dst_dir = proj.project_dir + "/Warped/"
if not os.path.exists(dst_dir):
    print "Notice: creating rubber sheeted texture directory =", dst_dir
    os.makedirs(dst_dir)

for image in proj.image_list:
    basename, ext = os.path.splitext(image.name)
    dst = dst_dir + basename + ".png"
    #if os.path.exists(dst):
    #    continue
    # print image.name
    scale = float(image.width) / float(camw)
Esempio n. 22
0
    def setup_pnp_problem(self, matches, idx, init_wgs, C_nav_v):
        """
        If you've got 99 problems, give yourself one more
        """
        K = self._PnP__camera_matrix

        keypoints = matches.keypoints[idx, :]
        img_pts = self.undistort_keypoints(keypoints)

        # First we need to create a local level Cartesian frame (NED),
        lon_lat_h = matches.world_coordinates[idx, :]
        ref = lon_lat_h.mean(0)
        ref[2] = ref[2] - 150.0
        world_n = navpy.lla2ned(lon_lat_h[:, 1], lon_lat_h[:, 0],
                                lon_lat_h[:, 2], ref[1], ref[0], ref[2])

        if self._PnP__db_bias is not None:
            world_n = world_n - self._PnP__db_bias

        t_n = navpy.lla2ned(init_wgs[0], init_wgs[1], init_wgs[2], ref[1],
                            ref[0], ref[2])

        C_nav_b = np.dot(self.C_b_v, C_nav_v.T).T
        R_c_w = np.dot(self.C_b_cam.T, C_nav_b.T)

        X_hat = world_n.T
        x_img = img_pts.T

        #Setup Constrained PnP
        metadata = {'K': K, 'R_c_w': R_c_w}
        prob = Problem()
        trans_state = StateBlock('translation', t_n.flatten())
        world_pt_states = [
            StateBlock('pt_%d' % ii, X_hat[:, ii].flatten())
            for ii in np.arange(X_hat.shape[1])
        ]
        prob.add_state_block(trans_state)
        for state in world_pt_states:
            prob.add_state_block(state)
        resids = []

        for ii in np.arange(keypoints.shape[0]):
            x_i = x_img[:, ii].T
            states_measured = ['translation', 'pt_%d' % ii]
            resid = ResidualBlock(states_measured,
                                  image_point_translation_worldpt_meas_func,
                                  image_point_trans_worldpt_jacobian_func)
            resid.set_meas(x_i, 4.0 * np.eye(2))
            resid.update_metadata(metadata)
            resids.append(resid)
            prob.add_residual_block(resid)

        # Add in Ground Control Meas....
        for ii in np.arange(keypoints.shape[0]):
            X_w = X_hat[:, ii].T
            states_measured = ['pt_%d' % ii]
            resid = ResidualBlock(states_measured, worldpt_meas_func,
                                  worldpt_jac_func)
            resid.set_meas(X_w, np.diag([25.0, 25.0, 25.0]))
            resid.update_metadata(None)
            resids.append(resid)
            prob.add_residual_block(resid)

        niter = prob.solve()

        R = np.array([])
        for r in resids:
            R = np.append(R, np.diag(r.R))
        R = np.diag(R)
        try:
            cov = np.linalg.pinv(
                np.dot(prob.J.T, np.dot(np.linalg.inv(R), prob.J)))
            pnp_n = prob.states['translation'].get_current_val()
            cov_n = np.abs(np.diag(cov)[0:3])
            pnp_wgs = np.array(navpy.ned2lla(pnp_n, ref[1], ref[0], ref[2]))
            return pnp_wgs, cov_n, niter
        except:
            return (np.array([]), np.array([]), 0)
async def run_mission(drone, mission_items, lla_ref, gz_sub):
    max_speed = 2  # m/s
    done = False  # Flag to signal when we're done with our mission

    # The ned values are slightly skewed because the lla reference is at the start location, not world origin
    # lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
    lla_ref_off = [0, 0, 0]

    [lat, lon, alt] = lla_ref  # Set default mission item to start location

    mission_item_idx = 0  # Keep track which mission item we're currently on

    async for mission_progress in drone.mission.mission_progress():
        if (not mission_progress.current == -1):
            print(f"Mission progress: "
                  f"{mission_progress.current}/"
                  f"{mission_progress.total}")

            # Processing one point at a time, when have reached the end of the current set of waypoints
            if (mission_progress.current == mission_progress.total - 1
                    and not done):

                print("-- Pause and clear mission")
                # Pause mission while calculating
                await drone.mission.pause_mission()
                await drone.mission.clear_mission()

                # Get current mission item index
                mission_item_idx = mission_progress.current

                print("-- Get Lidar Readings")
                # Get Lidar readings
                lsr_val = await gz_sub.get_LaserScanStamped()
                print("-- Register readings")
                registered_scans = get_world_obst(lsr_val, lla_ref_off)
                print(registered_scans)

                print("-- Get new lla")
                ####### Replace this code with your algorithm
                if (mission_item_idx == 0):
                    [lat, lon, alt] = navpy.ned2lla([0, 0, -1], lla_ref[0],
                                                    lla_ref[1], lla_ref[2])
                elif (mission_item_idx == 1):
                    [lat, lon, alt] = navpy.ned2lla([0, 10, -3], lla_ref[0],
                                                    lla_ref[1], lla_ref[2])
                elif (mission_item_idx == 2):
                    [lat, lon, alt] = navpy.ned2lla([0, 15, -5.5], lla_ref[0],
                                                    lla_ref[1], lla_ref[2])
                elif (mission_item_idx == 3):
                    [lat, lon, alt] = navpy.ned2lla([0, 30, -5.5], lla_ref[0],
                                                    lla_ref[1], lla_ref[2])
                ####### Replace this code with your algorithm
                else:
                    # Make sure that you set done to True when your algorithm has reached the endpoint
                    # and the drone is ready to land
                    done = True

                # Insert the mission item created into the overall mission
                print("-- Making mission plan")
                mission_items.insert(
                    mission_item_idx,
                    MissionItem(lat, lon, alt, max_speed, True, float('nan'),
                                float('nan'), MissionItem.CameraAction.NONE,
                                float('nan'), float('nan')))
                mission_plan = MissionPlan(mission_items)

                # Upload updated mission to the drone
                print("-- Uploading updated mission")
                await drone.mission.upload_mission(mission_plan)

                # Wait for the upload to finish, otherwise it can't resume
                await asyncio.sleep(1)

                # Start the mission from the item that we just added
                print("-- Resuming mission")
                await drone.mission.set_current_mission_item(mission_item_idx)
                await drone.mission.start_mission()

        # Land the drone at the end location
        if (mission_progress.current == mission_progress.total and done):
            # Get Lidar readings
            lsr_val = await gz_sub.get_LaserScanStamped()
            registered_scans = get_world_obst(lsr_val, lla_ref_off)
            print(registered_scans)
            print("-- Returning Home and Landing")
            await drone.action.land()
Esempio n. 24
0
    def make_interpolator(self, lla_ref, width_m, height_m, step_m):
        print "Notice: constructing NED area interpolator"
        rows = (height_m / step_m) + 1
        cols = (width_m / step_m) + 1
        #ned_pts = np.zeros((cols, rows))
        #for r in range(0,rows):
        #    for c in range(0,cols):
        #        idx = (cols*r)+c
        #        #va = self.srtm_z[idx]
        #        #if va == 65535 or va < 0 or va > 10000:
        #        #    va = 0.0
        #        #z = va
        #        ned_pts[c,r] = 0.0
        
        # build regularly gridded x,y coordinate list and ned_pts array
        n_list = np.linspace(-height_m*0.5, height_m*0.5, rows)
        e_list = np.linspace(-width_m*0.5, width_m*0.5, cols)
        #print "e's:", e_list
        #print "n's:", n_list
        ned_pts = []
        for e in e_list:
            for n in n_list:
                ned_pts.append( [n, e, 0] )

        # convert ned_pts list to lla coordinates (so it's not
        # necessarily an exact grid anymore, but we can now
        # interpolate elevations out of the lla interpolators for each
        # tile.
        navpy_pts = navpy.ned2lla(ned_pts, lla_ref[0], lla_ref[1], lla_ref[2])
        #print navpy_pts
        
        # build list of (lat, lon) points for doing actual lla
        # elevation lookup
        ll_pts = []
        for i in range( len(navpy_pts[0]) ):
            lat = navpy_pts[0][i]
            lon = navpy_pts[1][i]
            ll_pts.append( [ lon, lat ] )
        #print "ll_pts:", ll_pts

        # set all the elevations in the ned_ds list to the extreme
        # minimum value. (rows,cols) might seem funny, but (ne)d is
        # reversed from (xy)z ... don't think about it too much or
        # you'll get a headache. :-)
        ned_ds = np.zeros((rows,cols))
        ned_ds[:][:] = -32768
        #print "ned_ds:", ned_ds
        
        # for each tile loaded, interpolate as many elevation values
        # as we can, then copy the good values into ned_ds.  When we
        # finish all the loaded tiles, we should have elevations for
        # the entire range of points.
        for tile in self.tile_dict:
            zs = self.tile_dict[tile].lla_interpolate(np.array(ll_pts))
            #print zs
            # copy the good altitudes back to the corresponding ned points
            for r in range(0,rows):
                for c in range(0,cols):
                    idx = (rows*c)+r
                    if zs[idx] > -10000:
                        ned_ds[r,c] = zs[idx]

        # quick sanity check
        for r in range(0,rows):
            for c in range(0,cols):
                idx = (rows*c)+r
                if ned_ds[r,c] < -10000:
                    print "Problem interpolating elevation for:", ll_pts[idx]
                    ned_ds[r,c] = 0.0
        #print "ned_ds:", ned_ds

        # now finally build the actual grid interpolator with evenly
        # spaced ned n, e values and elevations interpolated out of
        # the srtm lla interpolator.
        self.interp = scipy.interpolate.RegularGridInterpolator((n_list, e_list), ned_ds, bounds_error=False, fill_value=-32768)

        do_plot = False
        if do_plot:
            imshow(ned_ds, interpolation='bilinear', origin='lower', cmap=cm.gray, alpha=1.0)
            grid(False)
            show()
        
        do_test = False
        if do_test:
            for i in range(40):
                ned = [(random.random()-0.5)*height_m,
                       (random.random()-0.5)*width_m,
                       0.0]
                lla = navpy.ned2lla(ned, lla_ref[0], lla_ref[1], lla_ref[2])
                #print "ned=%s, lla=%s" % (ned, lla)
                nedz = self.interp([ned[0], ned[1]])
                tile = make_tile_name(lla[0], lla[1])
                llaz = self.tile_dict[tile].lla_interpolate(np.array([lla[1], lla[0]]))
                print "nedz=%.2f llaz=%.2f" % (nedz, llaz)
Esempio n. 25
0
 def coverage_lla(self, ref):
     xmin, ymin, xmax, ymax = self.coverage_xy()
     minlla = navpy.ned2lla([ymin, xmin, 0.0], ref[0], ref[1], ref[2])
     maxlla = navpy.ned2lla([ymax, xmax, 0.0], ref[0], ref[1], ref[2])
     return (minlla[1], minlla[0], maxlla[1], maxlla[0])
Esempio n. 26
0
            bins[index].append(np.array(ned))
        else:
            bins[index] = [np.array(ned)]

    for index in bins:
        sum = np.zeros(3)
        for p in bins[index]:
            sum += p
        avg = sum / len(bins[index])
        print(index, len(bins[index]), avg)

    # write out simple csv version
    filename = os.path.join(project_path, "ImageAnalysis", "zooniverse.csv")
    with open(filename, 'w') as f:
        fieldnames = ['id', 'lat_deg', 'lon_deg', 'alt_m', 'comment']
        writer = csv.DictWriter(f, fieldnames=fieldnames)
        writer.writeheader()
        for index in bins:
            sum = np.zeros(3)
            for p in bins[index]:
                sum += p
                avg = sum / len(bins[index])
            lla = navpy.ned2lla([avg], ref[0], ref[1], ref[2])
            tmp = {}
            tmp['id'] = index
            tmp['lat_deg'] = lla[0]
            tmp['lon_deg'] = lla[1]
            tmp['alt_m'] = lla[2]
            tmp['comment'] = "zooniverse"
            writer.writerow(tmp)
Esempio n. 27
0
async def run_mission(drone, mission_items, lla_ref, gz_sub):
    done = False  # Flag to signal when we're done with our mission

    # The ned values are slightly skewed because the lla reference is at the start location, not world origin
    # lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
    lla_ref_off = [0, 0, 0]

    [lat, lon, alt] = lla_ref  # Set default mission item to start location

    mission_item_idx = 0  # Keep track which mission item we're currently on

    async for mission_progress in drone.mission.mission_progress():
        if (not mission_progress.current == -1):
            print(f"Mission progress: "
                  f"{mission_progress.current}/"
                  f"{mission_progress.total}")

            # Processing one point at a time, when have reached the end of the current set of waypoints
            if (mission_progress.current == mission_progress.total - 1
                    and not done):

                print("-- Pause and clear mission")
                # Pause mission while calculating
                await drone.mission.pause_mission()
                await drone.mission.clear_mission()

                # Get current mission item index
                mission_item_idx = mission_progress.current

                print("-- Get Lidar Readings")
                # Get Lidar readings
                lsr_val = await gz_sub.get_LaserScanStamped()

                # Get next wp and determine if done
                print("-- Getting next wp")
                [x, y, z, speed] = get_next_wp(lsr_val, lla_ref_off)
                if (not np.isnan(x)):
                    [lat, lon, alt] = navpy.ned2lla([y, x, -z], lla_ref[0],
                                                    lla_ref[1], lla_ref[2])
                else:
                    print("-- Mission is done")
                    done = True

                # Insert the mission item created into the overall mission
                print("-- Making mission plan")
                mission_items.insert(
                    mission_item_idx,
                    MissionItem(lat, lon, alt, speed, True, float('nan'),
                                float('nan'), MissionItem.CameraAction.NONE,
                                float('nan'), float('nan')))
                mission_plan = MissionPlan(mission_items)

                # Upload updated mission to the drone
                print("-- Uploading updated mission")
                await drone.mission.upload_mission(mission_plan)

                # Wait for the upload to finish, otherwise it can't resume
                await asyncio.sleep(1)

                # Start the mission from the item that we just added
                print("-- Resuming mission")
                await drone.mission.set_current_mission_item(mission_item_idx)
                await drone.mission.start_mission()

        # Land the drone at the end location
        if (mission_progress.current == mission_progress.total and done):
            print("-- Landing")
            await drone.action.land()
async def run():
    # Some of the values are inf and numpy complains when doing math with them, so turn off warnings
    np.seterr(all='ignore')
    np.set_printoptions(suppress=True)

    # Make the Gazebo subscriber object
    gz_sub = GazeboMessageSubscriber(HOST, PORT)

    # Actually do the subscription
    gz_task = asyncio.ensure_future(gz_sub.connect())

    # Get a test message - this will speed up all of the subsequent requests
    await gz_sub.get_LaserScanStamped()

    # Initialize drone and connect
    drone = System()
    await drone.connect(system_address="udp://:14540")

    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"Drone discovered with UUID: {state.uuid}")
            break

    # Make sure telemetry is good so that we know we can trust the position and orientation values
    print("Waiting for drone to have a global position estimate...")
    async for health in drone.telemetry.health():
        if health.is_global_position_ok:
            print("Global position estimate ok")
            break

    # Get our starting position and use that as reference for conversions between lla and ned
    print("Fetching amsl altitude at home location....")
    lla_ref = []
    async for terrain_info in drone.telemetry.position():
        lla_ref = [
            terrain_info.latitude_deg, terrain_info.longitude_deg,
            terrain_info.absolute_altitude_m
        ]
        print(lla_ref)
        break

    # First mission item is to takeoff to just above our starting location
    mission_items = []
    mission_items.append(
        MissionItem(lla_ref[0], lla_ref[1], lla_ref[2] + 1, 1, True,
                    float('nan'), float('nan'), MissionItem.CameraAction.NONE,
                    float('nan'), float('nan')))

    # Last mission item is at the end of the terrain, just above the terrain
    # All of your mission items go between the the start point above and the end point below
    [lat, lon, alt] = navpy.ned2lla([0, 40, -1], lla_ref[0], lla_ref[1],
                                    lla_ref[2])
    mission_items.append(
        MissionItem(lat, lon, alt, 1, True, float('nan'), float('nan'),
                    MissionItem.CameraAction.NONE, float('nan'), float('nan')))

    mission_plan = MissionPlan(mission_items)

    # Spool up the mission and mission monitor
    mission_task = asyncio.ensure_future(
        run_mission(drone, mission_items, lla_ref, gz_sub))
    termination_task = asyncio.ensure_future(
        observe_is_in_air(drone, [mission_task, gz_task]))

    # Make the drone dangerous (arm it)
    print("-- Arming")
    await drone.action.arm()
    await asyncio.sleep(1)

    # Upload mission with our first item (more will be added in the mission)
    print("-- Uploading mission")
    await drone.mission.upload_mission(mission_plan)
    await asyncio.sleep(1)

    await drone.action.set_takeoff_altitude(lla_ref[2] + 1)
    # Start running through the mission
    # The mission task will watch the progression and add new points as we reach the old ones
    # Once the end condition is met, the drone will land
    print("-- Starting mission")
    await drone.mission.start_mission()

    await termination_task
Esempio n. 29
0
 def ned2lla(self, n, e, d):
     lla = navpy.ned2lla([n, e, d], self.ned_ref[0], self.ned_ref[1],
                         self.ned_ref[2])
     # print(n, e, d, lla)
     return lla
Esempio n. 30
0
    def build(self, cam_pos, view_size):
        alpha = 0.6
        depth = 0
        a1 = view_size / 20
        a2 = view_size / 5
        props = base.win.getProperties()
        y = props.getYSize()
        pxm = float(y) / view_size
        text_scale = 42 / y
        # print("y:", y, "view_size:", view_size, "pxm:", pxm)

        # center reticle
        ls = LineSegs()
        ls.setThickness(1)
        ls.setColor(0.0, 1.0, 0.0, alpha)
        ls.moveTo(cam_pos[0] + a1, cam_pos[1], 0)
        ls.drawTo(cam_pos[0] + a2, cam_pos[1], 0)
        ls.moveTo(cam_pos[0] - a1, cam_pos[1], 0)
        ls.drawTo(cam_pos[0] - a2, cam_pos[1], 0)
        ls.moveTo(cam_pos[0], cam_pos[1] + a1, 0)
        ls.drawTo(cam_pos[0], cam_pos[1] + a2, 0)
        ls.moveTo(cam_pos[0], cam_pos[1] - a1, 0)
        ls.drawTo(cam_pos[0], cam_pos[1] - a2, 0)
        self.node = NodePath(ls.create())
        self.node.setDepthTest(False)
        self.node.setDepthWrite(False)
        self.node.setBin("unsorted", depth)
        self.node.reparentTo(self.render)

        # measurement marker
        h_size = view_size * base.getAspectRatio()
        h = math.pow(2, int(round(math.log2(h_size / 10.0))))
        # print("h_size:", h_size, h)
        ls = LineSegs()
        ls.setThickness(2)
        ls.setColor(0.0, 1.0, 0.0, alpha)
        ls.moveTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.48 * view_size, 0)
        ls.drawTo(cam_pos[0] - 0.48 * h_size + h,
                  cam_pos[1] - 0.48 * view_size, 0)
        ls.moveTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.48 * view_size, 0)
        ls.drawTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.46 * view_size, 0)
        ls.moveTo(cam_pos[0] - 0.48 * h_size + h,
                  cam_pos[1] - 0.48 * view_size, 0)
        ls.drawTo(cam_pos[0] - 0.48 * h_size + h,
                  cam_pos[1] - 0.46 * view_size, 0)
        self.node1 = NodePath(ls.create())
        self.node1.setDepthTest(False)
        self.node1.setDepthWrite(False)
        self.node1.setBin("unsorted", depth)
        self.node1.reparentTo(self.render)
        if h >= 1.0:
            dist_text = "%.0f m" % (h)
        elif h >= 0.1:
            dist_text = "%.1f cm" % (h * 100)
        else:
            dist_text = "%.1f mm" % (h * 1000)
        self.text2 = OnscreenText(text=dist_text,
                                  pos=(-0.95 * base.getAspectRatio(), -0.94),
                                  scale=text_scale,
                                  fg=(0.0, 1.0, 0.0, 1.0),
                                  shadow=(0.1, 0.1, 0.1, 0.8),
                                  align=TextNode.ALeft)

        # position display
        z = self.surface.get_elevation(cam_pos[0], cam_pos[1])
        lla = navpy.ned2lla([cam_pos[1], cam_pos[0], z], self.ned_ref[0],
                            self.ned_ref[1], self.ned_ref[2])
        pos_str = "Lat: %.7f  Lon: %.7f  Alt(m): %.1f" % (lla[0], lla[1],
                                                          lla[2])
        self.text1 = OnscreenText(text=pos_str,
                                  pos=(0.95 * base.getAspectRatio(), -0.95),
                                  scale=text_scale,
                                  fg=(0.0, 1.0, 0.0, 1.0),
                                  shadow=(0.1, 0.1, 0.1, 0.8),
                                  align=TextNode.ARight)
Esempio n. 31
0
    def __opencv_pnp(self, new_matches, ref=None):
        """
        This function is a callback which listens for new\
        vision_nav_msgs/FeatureCorrespondence2D3D messages, and computes pose \
        (3D position and orientation) of the camera in the world frame \
        associated with new_matches.world_coordinate_frame. For now \
        we're assuming this is WGS-84, [Longitude (deg), Latitude (deg), \
        Height Above Ellipsoid (meters)]. Need to add logic in to transform\
        these points to some arbitrary frame later on.
        """

        if new_matches.num_correspondences > 5:
            keypoints = new_matches.keypoints
            lon_lat_h = new_matches.world_coordinates

            # First we need to create a local level Cartesian frame (NED),
            # and get the world_coordinates into that frame
            # Use the first world point as the reference for NED frame
            if ref is None:
                ref = lon_lat_h.mean(0)
                ref[2] = ref[2] - 150.0
            world_pts_ned = navpy.lla2ned(lon_lat_h[:, 1], lon_lat_h[:, 0],
                                          lon_lat_h[:,
                                                    2], ref[1], ref[0], ref[2])

            if self.__db_bias is not None:
                world_pts_ned = world_pts_ned - self.__db_bias

            # Apply the 2D geometric constraint
            idx = self.__apply_geometric_constraint(
                world_pts_ned[:, 0:2].astype(np.float32),
                keypoints.astype(np.float32))

            print("%d kp from Matcher :: %d passed geometric constraint" %
                  (new_matches.num_correspondences, idx.shape[0]))

            # Do PnP
            if idx.shape[0] > 6:
                success, rvec, tvec, pnp_status = \
                    cv2.solvePnPRansac(
                        world_pts_ned[idx, :].astype(np.float32).reshape(idx.shape[0], 1, 3),
                        keypoints[idx, :].astype(np.float32).reshape(idx.shape[0], 1, 2),
                        self.__camera_matrix,
                        self.__distortion,
                        reprojectionError=2.0)

                if (success) and (pnp_status.shape[0] >= 6):
                    # Rvec is a rodrigues vector from world to cam, so need to
                    # transpose
                    C_n_b = (cv2.Rodrigues(rvec)[0]).transpose()

                    # Then tvec is from world to cam, so need to rotate into
                    # world frame and negate
                    t_nav = -1 * np.dot(C_n_b, tvec.reshape(3, 1)).flatten()

                    t_wgs = navpy.ned2lla(t_nav, ref[1], ref[0], ref[2])
                    t_wgs = np.array(t_wgs)
                    return t_wgs, idx[pnp_status].flatten()
            else:
                print("Bailing Not Enough Matches")
        return np.array([]), np.array([])
Esempio n. 32
0
def enu2lla(enu, gps_ref, latlon_unit='deg', alt_unit='m', model='wgs84'):
    ned = np.dot(GlobalVals.C_ENU_NED, enu)
    lla = ned2lla(ned, gps_ref.lat, gps_ref.lon, gps_ref.alt, latlon_unit,
                  alt_unit, model)
    return lla
    proj_list = project.projectVectors(IK, image, corner_list, pose=args.pose)
    #print "proj_list:\n", proj_list
    if args.pose == 'direct':
        pts_ned = srtm.interpolate_vectors(image.camera_pose, proj_list)
    elif args.pose == 'sba':
        pts_ned = srtm.interpolate_vectors(image.camera_pose_sba, proj_list)
    # print "pts (ned):\n", pts_ned

    image.corner_list_ned = []
    image.corner_list_lla = []
    image.corner_list_xy = []
    for ned in pts_ned:
        #print p
        image.corner_list_ned.append([ned[0], ned[1]])
        image.corner_list_lla.append(
            navpy.ned2lla([ned], ref[0], ref[1], ref[2]))
        image.corner_list_xy.append([ned[1], ned[0]])

dst_dir = proj.project_dir + "/Warped/"
if not os.path.exists(dst_dir):
    print "Notice: creating rubber sheeted texture directory =", dst_dir
    os.makedirs(dst_dir)

for image in proj.image_list:
    basename, ext = os.path.splitext(image.name)
    dst = dst_dir + basename + ".png"
    #if os.path.exists(dst):
    #    continue
    # print image.name
    scale = float(image.width) / float(camw)
    K = proj.cam.get_K(scale)
Esempio n. 34
0
 def ConvNED2LLA(self, NED):
     # NED should be 1X3 array
     lat, lon, alt = navpy.ned2lla(NED, self.LLAref[0], self.LLAref[1], self.LLAref[2])
     return lat, lon, alt
Esempio n. 35
0
    def make_interpolator(self, lla_ref, width_m, height_m, step_m):
        print("Notice: constructing NED area interpolator")
        rows = int(height_m / step_m) + 1
        cols = int(width_m / step_m) + 1
        #ned_pts = np.zeros((cols, rows))
        #for r in range(0,rows):
        #    for c in range(0,cols):
        #        idx = (cols*r)+c
        #        #va = self.srtm_z[idx]
        #        #if va == 65535 or va < 0 or va > 10000:
        #        #    va = 0.0
        #        #z = va
        #        ned_pts[c,r] = 0.0

        # build regularly gridded x,y coordinate list and ned_pts array
        n_list = np.linspace(-height_m * 0.5, height_m * 0.5, rows)
        e_list = np.linspace(-width_m * 0.5, width_m * 0.5, cols)
        #print "e's:", e_list
        #print "n's:", n_list
        ned_pts = []
        for e in e_list:
            for n in n_list:
                ned_pts.append([n, e, 0])

        # convert ned_pts list to lla coordinates (so it's not
        # necessarily an exact grid anymore, but we can now
        # interpolate elevations out of the lla interpolators for each
        # tile.
        navpy_pts = navpy.ned2lla(ned_pts, lla_ref[0], lla_ref[1], lla_ref[2])
        #print navpy_pts

        # build list of (lat, lon) points for doing actual lla
        # elevation lookup
        ll_pts = []
        for i in range(len(navpy_pts[0])):
            lat = navpy_pts[0][i]
            lon = navpy_pts[1][i]
            ll_pts.append([lon, lat])
        #print "ll_pts:", ll_pts

        # set all the elevations in the ned_ds list to the extreme
        # minimum value. (rows,cols) might seem funny, but (ne)d is
        # reversed from (xy)z ... don't think about it too much or
        # you'll get a headache. :-)
        ned_ds = np.zeros((rows, cols))
        ned_ds[:][:] = -32768
        #print "ned_ds:", ned_ds

        # for each tile loaded, interpolate as many elevation values
        # as we can, then copy the good values into ned_ds.  When we
        # finish all the loaded tiles, we should have elevations for
        # the entire range of points.
        for tile in self.tile_dict:
            zs = self.tile_dict[tile].lla_interpolate(np.array(ll_pts))
            #print zs
            # copy the good altitudes back to the corresponding ned points
            for r in range(0, rows):
                for c in range(0, cols):
                    idx = (rows * c) + r
                    if zs[idx] > -10000:
                        ned_ds[r, c] = zs[idx]

        # quick sanity check
        for r in range(0, rows):
            for c in range(0, cols):
                idx = (rows * c) + r
                if ned_ds[r, c] < -10000:
                    print("Problem interpolating elevation for:", ll_pts[idx])
                    ned_ds[r, c] = 0.0
        #print "ned_ds:", ned_ds

        # now finally build the actual grid interpolator with evenly
        # spaced ned n, e values and elevations interpolated out of
        # the srtm lla interpolator.
        self.interp = scipy.interpolate.RegularGridInterpolator(
            (n_list, e_list), ned_ds, bounds_error=False, fill_value=-32768)

        do_plot = False
        if do_plot:
            imshow(ned_ds,
                   interpolation='bilinear',
                   origin='lower',
                   cmap=cm.gray,
                   alpha=1.0)
            grid(False)
            show()

        do_test = False
        if do_test:
            for i in range(40):
                ned = [(random.random() - 0.5) * height_m,
                       (random.random() - 0.5) * width_m, 0.0]
                lla = navpy.ned2lla(ned, lla_ref[0], lla_ref[1], lla_ref[2])
                #print "ned=%s, lla=%s" % (ned, lla)
                nedz = self.interp([ned[0], ned[1]])
                tile = make_tile_name(lla[0], lla[1])
                llaz = self.tile_dict[tile].lla_interpolate(
                    np.array([lla[1], lla[0]]))
                print("nedz=%.2f llaz=%.2f" % (nedz, llaz))
Esempio n. 36
0
 def coverage_lla(self, ref):
     xmin, ymin, xmax, ymax = self.coverage_xy()
     minlla = navpy.ned2lla([ymin, xmin, 0.0], ref[0], ref[1], ref[2])
     maxlla = navpy.ned2lla([ymax, xmax, 0.0], ref[0], ref[1], ref[2])
     return (minlla[1], minlla[0], maxlla[1], maxlla[0])