final.py

import os
import math
import numpy as np
import pandas as pd
from PIL import Image
from collections import namedtuple

def preprocessing(folder):
    '''read data and obtain point cloud and camera center.'''
    filename = os.path.join(folder, 'final_project_point_cloud.csv')
    point_clouds = pd.read_csv(filename, sep = ',', header = None, names = ['combined'])
    
    # split the combined column into separate columns
    point_clouds = point_clouds['combined'].str.split(' ', expand = True).astype(np.float64)
    point_clouds.columns = ['latitude', 'longitude', 'altitude', 'intensity']
    
    filename = os.path.join(folder, 'image', 'camera.txt')
    with open(filename, 'r') as f:
        f.readline()
        camera_file = f.readline()
        
    camera_center = [float(camera) for camera in camera_file.split(', ')]
    camera_df = pd.DataFrame([camera_center[:3]], 
                             columns = ['latitude', 'longitude', 'altitude'])
    
    camera_df['latitude_radian'] = camera_df['latitude'].apply(convert_degree_to_radian)
    camera_df['longitude_radian'] = camera_df['longitude'].apply(convert_degree_to_radian)
    point_clouds['latitude_radian'] = point_clouds['latitude'].apply(convert_degree_to_radian)
    point_clouds['longitude_radian'] = point_clouds['longitude'].apply(convert_degree_to_radian)
    return point_clouds, camera_center, camera_df
    
def transform(point_clouds, camera_df):
    '''coordinate transformation
    earth's general information
    a = earth's equatorial radian 
    b = earth's polar radian
    f = earth's flattening f
    e = eccentricity
    '''
    a = 6378137
    b = 6356752.3
    f = (a - b) / a
    e = math.sqrt(f * (2 - f))
    earth_ellipsoid = namedtuple('earth_ellipsoid', ['a', 'b', 'e'])
    earth = earth_ellipsoid(a, b, e)
    
    altitude = camera_df['altitude'].values
    latitude = camera_df['latitude_radian'].values
    longitude = camera_df['longitude_radian'].values
    x0, y0, z0 = lla_to_ecef(latitude, longitude, altitude, earth)
    
    altitude = point_clouds['altitude'].values
    latitude = point_clouds['latitude_radian'].values
    longitude = point_clouds['longitude_radian'].values
    x, y, z = lla_to_ecef(latitude, longitude, altitude, earth)
    
    ecef = pd.DataFrame( np.column_stack((x, y, z)), columns = ['x', 'y', 'z'] )
    ecef[['latitude_radian', 'longitude_radian']] = point_clouds[['latitude_radian', 'longitude_radian']]

    R = create_rotation_matrix(camera_df)
    ecef['enu'] = ecef.apply(ecef_to_enu, axis = 1, args = (R, x0, y0, z0))
    # create 3d point cloud obj file
    create_obj_file(ecef['enu'], 'enu.obj', 3, point_clouds['intensity'])
    # the 30 degree rotation was hard-coded, which was 
    # done by plotting the visualization and see what degree of the
    # rotation best matches the camera coordinate
    R = np.array([[math.sqrt(3) / 2, -0.5], [0.5, math.sqrt(3) / 2]])
            
    
    ecef['camera_coord'] = ecef['enu'].apply(lambda row: enu_to_camera(row, R))
    camera_coordinate = np.vstack(ecef['camera_coord'])
    
    x = camera_coordinate[:, 0]
    y = camera_coordinate[:, 2]
    z = camera_coordinate[:, 1]
    t = point_clouds['intensity']
    camera_coordinate1 = np.vstack((x, y, z, t))
    # create 3d point cloud obj file for camera coordinate
    cam_coord = pd.DataFrame(camera_coordinate1[:, 0:3])
    create_obj_file(cam_coord, 'cam_coord.obj', 3, point_clouds['intensity'])
    camera_coordinate1 = camera_coordinate1.T
    return camera_coordinate1, x, y, z
    
def slice_data(camera_coordinate1, x, y, z):
    '''slicing the point cloud to four part, each part corresponding to one image'''
    # Slice 3D space for FRONT orientation
    cc_x = x
    cc_y = y
    cc_z = z
    front = get_orientation_slice(camera_coordinate1, cc_x, cc_y, cc_z)
    
    # Slice 3D space for BACK orientation
    cc_x = -x
    cc_y = y
    cc_z = -z
    back = get_orientation_slice(camera_coordinate1, cc_x, cc_y, cc_z)
    
    # Slice 3D space for LEFT orientation
    cc_x = z
    cc_y = y
    cc_z = -x
    left = get_orientation_slice(camera_coordinate1, cc_x, cc_y, cc_z)
    
    # Slice  3D space for RIGHT orientation
    cc_x = -z
    cc_y = y
    cc_z = x
    right = get_orientation_slice(camera_coordinate1, cc_x, cc_y, cc_z)
    return front, back, left, right
    
def adjust_image_direction(front, back, left, right):
    '''rotation the point cloud coordinate so that z-axis will be front view of the photo
    '''
    left = pd.DataFrame(left)
    right = pd.DataFrame(right)
    front = pd.DataFrame(front)
    back = pd.DataFrame(back)
    front_directed = front
    back_directed = pd.DataFrame()
    back_directed[0] = -back[0]
    back_directed[1] = back[1]
    back_directed[2] = -back[2]
    right_directed = pd.DataFrame()
    right_directed[0] = -right[2]
    right_directed[1] = right[1]
    right_directed[2] = right[0]
    left_directed = pd.DataFrame()
    left_directed[0] = left[2]
    left_directed[1] = left[1]
    left_directed[2] = -left[0]

    return front_directed, back_directed, left_directed, right_directed
    
def point_cloud_to_images(folder,front, back, left, right):
    '''getting the image coordinate of point cloud'''
    image_path = os.path.join(folder, 'image', 'front.jpg')
    front_image = Image.open(image_path)
    front_resolution = front_image.size[0] * front_image.size[1]
    resolution = math.sqrt(front_resolution)
    
    front_directed, back_directed, left_directed, right_directed = \
                              adjust_image_direction(front, back, left, right)

    front_image_coordinate = pd.DataFrame()
    front_image_coordinate['x'] = front_directed.apply(lambda row: camera2image_x(row, resolution), axis=1)
    front_image_coordinate['y'] = front_directed.apply(lambda row: camera2image_y(row, resolution), axis=1)
    front_image_coordinate['t'] = pd.DataFrame(front)[3]
    create_obj_file(front_image_coordinate, 'front_image.obj', 2, front_image_coordinate['t'])
    
    back_image_coordinate = pd.DataFrame()
    back_image_coordinate['x'] = back_directed.apply(lambda row: camera2image_x(row, resolution), axis=1)
    back_image_coordinate['y'] = back_directed.apply(lambda row: camera2image_y(row, resolution), axis=1)
    back_image_coordinate['t'] = pd.DataFrame(back)[3]
    create_obj_file(back_image_coordinate, 'back_image.obj', 2, back_image_coordinate['t'])
    
    
    right_image_coordinate = pd.DataFrame()
    right_image_coordinate['x'] = right_directed.apply(lambda row: camera2image_x(row, resolution), axis=1)
    right_image_coordinate['y'] = right_directed.apply(lambda row: camera2image_y(row, resolution), axis=1)
    right_image_coordinate['t'] = pd.DataFrame(right)[3]
    create_obj_file(right_image_coordinate, 'right_image.obj', 2, right_image_coordinate['t'])
    
    
    left_image_coordinate = pd.DataFrame()
    left_image_coordinate['x'] = left_directed.apply(lambda row: camera2image_x(row, resolution), axis=1)
    left_image_coordinate['y'] = left_directed.apply(lambda row: camera2image_y(row, resolution), axis=1)
    left_image_coordinate['t'] = pd.DataFrame(left)[3]
    create_obj_file(left_image_coordinate, 'left_image.obj', 2, left_image_coordinate['t'])
    
    return front_image_coordinate, back_image_coordinate,right_image_coordinate,left_image_coordinate

def convert_degree_to_radian(degree):
    """convert degree to radian"""
    radian = degree * math.pi / 180
    return radian


def lla_to_ecef(latitude, longitude, altitude, earth, degree = False):
    """transform LLA to ECEF coordinate"""
    a = earth.a
    e = earth.e
    if degree:
        latitude = np.radians(latitude)
        longitude = np.radians(longitude)

    # compute N, distance from surface to z axis
    N = a / np.sqrt(1 - e ** 2 * np.sin(latitude) ** 2)
    
    # perform transformation to x, y, z axis according to the formula
    x = (altitude + N) * np.cos(latitude) * np.cos(longitude)
    y = (altitude + N) * np.cos(latitude) * np.sin(longitude)
    z = (altitude + (1 - e ** 2) * N) * np.sin(latitude)
    return x, y, z


def create_rotation_matrix(row):
    """
    create the rotation matrix for each point cloud
    to transform it to the position with respect to
    the camera's perspective, instead of with respect
    to the earth's origin
    """
    latitude_radian = row['latitude_radian']
    longitude_radian = row['longitude_radian']
    lat_sin = math.sin(latitude_radian)
    lat_cos = math.cos(latitude_radian)
    lon_sin = math.sin(longitude_radian)
    lon_cos = math.cos(longitude_radian)
    R = np.array([
        [-lon_sin, lon_cos, 0],
        [-lon_cos * lat_sin, -lat_sin * lon_sin, lat_cos],
        [lat_cos * lon_cos, lat_cos * lon_sin, lat_sin]
    ])
    return R


def ecef_to_enu(row, R, x0, y0, z0):
    """coordinate transformation for each point cloud"""
    coord_diff = np.array([row['x'] - x0, row['y'] - y0, row['z'] - z0]).reshape(3, 1)
    result = R.dot(coord_diff).ravel().tolist()
    return result

def enu_to_camera(row, R):
    """rotate enu to match the camera"""
    coord = np.array([row[0], row[1]]).reshape(2, 1)
    result = R.dot(coord).ravel().tolist()
    result.append(row[2])
    return result

def get_orientation_slice(cc, x, y, z):
    orientation_slice = cc[(z > np.abs(x)) & (z > np.abs(y))]
    return orientation_slice


# convert 3d camera coordinate to 2d image coordinate
def camera2image_x(row, resolution):
    return row[1] / row[2] * float(resolution - 1) / 2 + float(resolution + 1) / 2

def camera2image_y(row, resolution):
    return row[0] / row[2] * float(resolution - 1) / 2 + float(resolution + 1) / 2

def hist_stretch(intensity):
    """ Normalize intensity values by stretching the histogram """
    
    # We take a subset of the point cloud values by removing very high intensity points
    int_ind = np.argsort(intensity)[:-200000]
    new_pc = intensity[int_ind]
    min_intensity = np.min(new_pc)
    max_intensity = np.max(new_pc)
    #print(min_intensity, max_intensity)
    
    new_pc = new_pc.apply(lambda x: (x - min_intensity) // (max_intensity - min_intensity) * 255)
    intensity[int_ind] = new_pc
    return(intensity)
    
def create_obj_file(df, filename, dimension, intensity_values):
    obj_df = pd.DataFrame()

    if(dimension == 3):
        obj_df[['x', 'y', 'z']] = df.apply(pd.Series)
    else:
        obj_df[['x', 'y']] = df.ix[:,0:2].apply(pd.Series)
    
    obj_df['intensity'] = intensity_values
    obj_df['v'] = 'v'
    
    if(dimension == 3):
        obj_df = obj_df[['v', 'x', 'y', 'z', 'intensity']]
    else:
        obj_df = obj_df[['v', 'x', 'y', 'intensity']]

    obj_df.to_csv(filename, sep=' ', index=False, header=False)
    
def main():
    folder = 'final_project_data'
    point_clouds, camera_center, camera_df = preprocessing(folder)
    point_clouds['intensity'] = hist_stretch(point_clouds['intensity'])
    camera_coordinate1,x,y,z = transform(point_clouds, camera_df)
    
    front, back, left, right = slice_data(camera_coordinate1,x,y,z)
    front_image_coordinate, back_image_coordinate,right_image_coordinate,left_image_coordinate = \
    point_cloud_to_images(folder,front, back, left, right)

if __name__ == "__main__":
    main()