This repository contains all the code for Parsing, Transforming and Training Multimodal Deep Learning Network, for Social Robot Navigation.
import numpy as np
import torch

def clip_velocities(gt_cmd_vel):        
    gt_cmd_vel[:,0] = np.clip(gt_cmd_vel[:,0], 0, 1.6)
    gt_cmd_vel[:,1] = np.clip(gt_cmd_vel[:,1], -0.55, 0.55)
    return gt_cmd_vel

def scale_min_max(gt_cmd_vel):
    gt_cmd_vel[:,0] = np.interp(gt_cmd_vel[:,0], (0, 1.6), (0, 10000))
    gt_cmd_vel[:,1] = np.interp(gt_cmd_vel[:,1], (-0.55, 0.55), (2*10**7, 4*10**7))
    return gt_cmd_vel

def reverse_scale(gt_cmd_vel):
    gt_cmd_vel[:,0] = np.interp(gt_cmd_vel[:,0], (0, 10000), (0, 1.6))
    gt_cmd_vel[:,1] = np.interp(gt_cmd_vel[:,1], (2*10**7, 4*10**7), (-0.55, 0.55))
    return gt_cmd_vel

def reverse_transform(cmd_vel_tensor):
    cmd_vel = cmd_vel_tensor.cpu().numpy()
    # print(cmd_vel.shape)
    cmd_scale_reversed = reverse_scale(cmd_vel)
    cmd_scale_reversed_tensor = torch.tensor(cmd_scale_reversed, dtype=torch.float32)
    return cmd_scale_reversed_tensor

def transform_cmd_vel(gt_cmd_vel): 
    clipped = clip_velocities(gt_cmd_vel)
    scaled_cmd_vel = scale_min_max(clipped)    
    return scaled_cmd_vel

def transform_to_gt_scale(cmd_vel_tensor, device):
    return reverse_transform(cmd_vel_tensor).to(device) 
scripts/data_builder/cmd_scaler.py
RobotiXX/multimodal-fusion-network
import numpy as np
import matplotlib.pyplot as plt

# Define the mean and standard deviation of the Gaussian distribution


def get_gaussian_weights(mean, std_dev):
    mean = 2.5
    std_dev = 1.4
    domain = np.array([i for i in range(0,12)])

    range_values = (1 / (std_dev * np.sqrt(2 * np.pi))) * np.exp(-0.5 * ((domain - mean) / std_dev)**2)
    
    # Output the values at integer points
    # for i in range(range_values.shape[0]):    
    #     print("Value at x =", i, "is", np.floor(range_values[i]*1000)+100)
    
    round_weights = np.floor(range_values * 1000)
    reshapeed_weights  = np.expand_dims(round_weights, axis=0)

    return  reshapeed_weights

scripts/data_builder/gaussian_weights.py
RobotiXX/multimodal-fusion-network
import pickle
import coloredlogs, logging
import os

from torch.utils.data import Dataset
coloredlogs.install()

class IndexDataset(Dataset):

    def __init__(self, dir_path):        
        self.root_path = dir_path
        self.pickle_path = os.path.join(dir_path , 'snapshot.pickle')        
        
        logging.info(f'Parsing pickle file: {self.pickle_path}')
    
        with open(self.pickle_path, 'rb') as data:
            self.content = pickle.load(data)

        logging.info('Picklefile loaded')

        # Exclude keys that does not have a local goal [as the robot did not travel 10 meters]
        keys = list(self.content.keys())
        for key in keys:
            if len(self.content[key]['local_goal'])!=12:                
                # print(key)
                self.content.pop(key)

    def __len__(self):
        # As images and point clouds will be in sets of 4
        return int(len(self.content.keys()) - 4)
    
    def __getitem__(self, offset_index) :
        # We are taking 4 sequential images, point clouds each time to account for temporal variation
        start_index = offset_index

        # Get data from respective index       
        gt_cmd_vel = self.content[start_index]['gt_cmd_vel']
        local_goal = self.content[start_index]['local_goal']
        robot_position = self.content[start_index]['robot_position']
        
        # Image paths
        image_paths = [ os.path.join(self.root_path, str(i)+'.jpg') for i in range(start_index, start_index+1) ]
        
        # only keep points that are under 5 + 1 (delta) meters from the robot
        point_clouds = []
        # print(list(self.content.keys()), start_index, end_index)
        for point_snapshot in range(start_index, start_index+1):
            filtered_points = []            
            for point in self.content[point_snapshot]['point_cloud']:
                if point[0] >= 0 and point[0] <= 8.009 and point[1]>=-3 and point[1]<=3 and point[2] >= 0.0299 and point[2] <= 2.0299:
                    filtered_points.append(point)                           

            point_clouds.append(filtered_points)                

        return (image_paths, point_clouds, local_goal, robot_position, gt_cmd_vel)
scripts/data_builder/indexer.py
RobotiXX/multimodal-fusion-network
import torch
from torchvision import transforms
import pickle
import numpy as np
import coloredlogs, logging
import os
import cv2
# import tf
import pyquaternion as pq

from torch.utils.data import Dataset
from scipy.spatial.transform import Rotation as R
from .transformer_pcl import get_voxelized_points
from .gaussian_weights import get_gaussian_weights
from .cmd_scaler import transform_cmd_vel


coloredlogs.install()




def read_images(path):
    # print(f"{path = }")
    image = cv2.imread(path)
    image_float = image.astype('float32')
    image_float /= 255.0
    # Will have to do some re-sizing
    return cv2.cvtColor(image_float, cv2.COLOR_BGR2RGB)

def get_transformation_matrix(position, quaternion):
    theta = R.from_quat(quaternion).as_euler('XYZ')[2]
    robo_coordinate_in_glob_frame  = np.array([[np.cos(theta), -np.sin(theta), position[0]],
                    [np.sin(theta), np.cos(theta), position[1]],
                    [0, 0, 1]])
    return robo_coordinate_in_glob_frame


class ApplyTransformation(Dataset):
    def __init__(self, input_data, grid_size = [72, 30, 30]):
        self.grid_size = np.asarray(grid_size)  
        self.input_data = input_data    
        self.image_transforms = transforms.Compose([
                    transforms.ToTensor(),
                    transforms.Resize((224,224),antialias=True)
            ])
    
    def __len__(self):
         # TODO: this will return 1 example set with the following details
        return len(self.input_data)

    def __getitem__(self, index):
        # Transform images
        data = self.input_data[index]
        self.image_paths = data[0]
        self.point_clouds = data[1]
        self.way_pts = data[2]        
        self.robot_position  = data[3]
        self.gt_cmd_vel = data[4]

        images = [ self.image_transforms(read_images(path)) for path in self.image_paths]
        stacked_images = torch.cat(images, dim=0)
        
        # Transform local goal into robot frame
        tf_matrix = get_transformation_matrix(self.robot_position[0],self.robot_position[1])     
        tf_inverse = np.linalg.pinv(tf_matrix)

        goals = np.concatenate([ np.array(self.way_pts), np.ones((12,1))], axis=1).transpose()
        

        all_pts = np.matmul(tf_inverse, goals) * get_gaussian_weights(2,1.3)
        all_pts = all_pts[:2, :]

        way_pts = all_pts[:, :4]
        local_goal = all_pts[:, 4]

        # print(f'{all_pts/150}')
        # print(f'{local_goal}')

        point_clouds = np.array(self.point_clouds[0])   
        point_clouds = get_voxelized_points(point_clouds)

        gt_cmd_vel = np.array([self.gt_cmd_vel[0], self.gt_cmd_vel[2]])
        gt_cmd_vel = np.around(gt_cmd_vel,2)
        gt_cmd_vel = np.expand_dims(gt_cmd_vel, axis=0)
        gt_cmd_vel = transform_cmd_vel(gt_cmd_vel)

        gt_pts = torch.tensor(way_pts, dtype=torch.float32).ravel()

        local_goal = torch.tensor(local_goal, dtype=torch.float32).ravel()        

        gt_cmd_vel = torch.tensor(gt_cmd_vel, dtype=torch.float32).ravel()        

        return (stacked_images, point_clouds, local_goal, gt_pts, gt_cmd_vel)




scripts/data_builder/transformer.py
RobotiXX/multimodal-fusion-network
import numpy as np
import torch
import torch.nn as nn


def scale_min_max(points,min_val=0, max_val=6):    

    x = np.expand_dims(np.array(points[:,0]), axis=1)    
    y = np.expand_dims(np.array(points[:,1]), axis=1)
    z = np.expand_dims(np.array(points[:,2]), axis=1)

    x = np.interp(x,(0, 8), (min_val, 8))
    y = np.interp(y,(-3, 3), (min_val, max_val))

    return np.concatenate([x,y,z], axis=1)



def get_voxelized_points(points_array):
    # Define the grid dimensions
    grid_size = 122  # Adjust based on your requirements

    # Create an empty voxel grid
    voxel_grid = np.zeros((162, grid_size, 42))    

    # Calculate the voxel size based on the grid dimensions
    voxel_size = 0.05

    if points_array.shape[0] == 0:
        input_tensor = torch.tensor(voxel_grid, dtype=torch.float32)
        input_tensor = input_tensor.unsqueeze(0)
        # print(f'returned_zeroed_array {input_tensor.shape}')
        return input_tensor

    # scale coordinate value
    points_array = scale_min_max(points_array)
    

    # Map points to the voxel grid
    grid_indices = np.floor(points_array / voxel_size).astype(int)    
    grid_indices = np.clip(grid_indices, np.array([0,0,0]), np.array([161,121,41]))

    # unique_indices, counts = np.unique(grid_indices, return_counts=True, axis=0)

    voxel_grid[grid_indices[:, 0], grid_indices[:, 1], grid_indices[:, 2]] = 1

    # Convert the voxel grid to a PyTorch tensor
    input_tensor = torch.tensor(voxel_grid, dtype=torch.float32)
    input_tensor = input_tensor.unsqueeze(0)  # Add batch and channel dimensions

    return input_tensor 
scripts/data_builder/transformer_pcl.py
RobotiXX/multimodal-fusion-network
from comet_ml import Experiment
import torch
import os
import numpy as np
from random import shuffle

from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from model_builder.multimodal.fusion_net import BcFusionModel
from torch.utils.data import DataLoader
from tqdm.auto import tqdm
import matplotlib.pyplot as plt
import coloredlogs, logging

from torch.optim.lr_scheduler import MultiStepLR

# Create an experiment with your api key
experiment = Experiment(
    api_key="Ly3Tc8W7kfxPmAxsArJjX9cgo",
    project_name= "multimodal-net-with-rnn",
    # project_name="kkk",
    workspace="bhabaranjan",
)

coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

min_val_error = 100000

val_dict = {}

def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=False)
    return data_loader

def get_loss(loss_fn, lin_vel, angular_vel, gt_lin, gt_angular, data_src):
    lin_error =  loss_fn(lin_vel, gt_lin) 
    anglr_error = loss_fn(angular_vel, gt_angular)
    error = lin_error + anglr_error
    
    lin_err_val = lin_error.item()
    anglr_error_val = anglr_error.item()

    experiment.log_metric(name = str('line_error_'+data_src), value=lin_err_val)
    experiment.log_metric(name = str('anglr_error_'+data_src), value=anglr_error_val)


    return error


def run_validation(val_files, model, batch_size, epoch, optim):
       print("Running Validation..\n")
       running_error = []
       loss = torch.nn.MSELoss()
       model.eval()
       with torch.no_grad():
        for val_file in val_files:        
            val_loader = None
            if val_file not in val_dict:
                val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
                val_dict[val_file] = val_loader
            else:
                val_loader = val_dict[val_file]

            per_file_loss_fusion  = []
            per_file_loss_ǐmage = []
            per_file_loss_pcl = []
            per_file_total_loss = []
            for index, (stacked_images, pcl ,local_goal, prev_cmd_vel, gt_cmd_vel) in tqdm(enumerate(val_loader)):
                stacked_images = stacked_images.to(device)
                pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                prev_cmd_vel= prev_cmd_vel.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                
                fsn_lin, fsn_anglr, img_lin, img_anglr, pcl_lin, pcl_anglr = model(stacked_images, pcl, local_goal, prev_cmd_vel)
                

                gt_x = torch.unsqueeze(gt_cmd_vel[:,0],1)
                gt_y = torch.unsqueeze(gt_cmd_vel[:,1],1)

                # print(fsn_lin)
                # print(fsn_anglr)
 
                # print(gt_x)
                # print(gt_y)
                
                error_fusion = get_loss(loss, fsn_lin, fsn_anglr, gt_x, gt_y,'fusion')
                error_img = get_loss(loss, img_lin, img_anglr, gt_x, gt_y, 'img')
                error_pcl = get_loss(loss, pcl_lin, pcl_anglr, gt_x, gt_y, 'pcl')
                
                error_total = error_fusion + ( 0.2 * error_img) + error_pcl

                per_file_loss_fusion.append(error_fusion.item())
                per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())                
                per_file_total_loss.append(error_total.item())
                
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_img'), value=np.average(per_file_loss_ǐmage), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl'), value=np.average(per_file_loss_pcl), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_fusion'), value=np.average(per_file_loss_fusion), epoch = epoch + 1)

            running_error.append(np.average(per_file_total_loss))

        avg_loss_on_validation = np.average(running_error)
        
        if (epoch+1) % 10 == 0 and epoch!=0:
            min_val_error = avg_loss_on_validation
            print(f"saving model weights at validation error {min_val_error}")
            torch.save({
            'model_state_dict': model.state_dict(),
            'optimizer_state_dict': optim.state_dict(),
            }, f'model_at_{epoch+1}.pth')

        print(f'=========================> Average Validation error is:   {avg_loss_on_validation} \n')
        return avg_loss_on_validation
            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = torch.nn.MSELoss()
    model = BcFusionModel()
    optim = torch.optim.Adam(model.parameters(), lr = 0.0001) 

    model.to(device)

    data_dict = {}
    for epoch in range(num_epochs):
        num_files = 0
        # lr = scheduler.get_last_lr()
        # experiment.log_metric( name = "Learning Rate Decay", value = lr, epoch= epoch+1)
        running_loss = []
        # shuffle(train_files)
        model.train()
        for train_file in train_files:        
            train_loader = None 
            print(train_file)
            if train_file not in data_dict:
                train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )   
                data_dict[train_file] = train_loader
            else:
                train_loader = data_dict[train_file]

            num_files += 1
            per_file_loss_fusion = [] 
            per_file_loss_ǐmage = [] 
            per_file_loss_pcl = [] 
            per_file_total_loss = []
            for index, (stacked_images, pcl ,local_goal, prev_cmd_vel, gt_cmd_vel) in enumerate(train_loader):

                # print(f'gt_cmd: {gt_cmd_vel}')
                # print(f'prev_cmd_vel:{prev_cmd_vel}')
                
                stacked_images = stacked_images.to(device)
                pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                prev_cmd_vel= prev_cmd_vel.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                # print(f"{pcl.shape = }")
                optim.zero_grad()
                
                fsn_lin, fsn_anglr, img_lin, img_anglr, pcl_lin, pcl_anglr = model(stacked_images, pcl, local_goal, prev_cmd_vel)
                
                gt_x = torch.unsqueeze(gt_cmd_vel[:,0],1)
                gt_y = torch.unsqueeze(gt_cmd_vel[:,1],1)

                # print(fsn_lin)
                # print(fsn_anglr)

                # print(gt_x)
                # print(gt_y)
                
                error_fusion = get_loss(loss, fsn_lin, fsn_anglr, gt_x, gt_y,'train_fusion')
                error_img = get_loss(loss, img_lin, img_anglr, gt_x, gt_y, 'train_img')
                error_pcl = get_loss(loss, pcl_lin, pcl_anglr, gt_x, gt_y,'train_pcl')
                
                error_total = error_fusion + error_img + error_pcl

                per_file_loss_fusion.append(error_fusion.item())
                per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())                
                per_file_total_loss.append(error_total.item())

                error_total.backward()
                optim.step()

                per_file_loss_fusion.append(error_fusion.item())
                per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())
                per_file_total_loss.append(error_total.item())

                print(f'step is:   {index} and total error is:   {error_total.item()}  image: {error_img.item()}  pcl: {error_pcl.item()} fusion: {error_fusion.item()}\n')
            
            experiment.log_metric(name = str(train_file.split('/')[-1]+ " mod:" +'img'), value=np.average(per_file_loss_ǐmage), epoch= epoch+1)
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'pcl'), value=np.average(per_file_loss_pcl), epoch= epoch+1)
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'fusion'), value=np.average(per_file_loss_fusion), epoch= epoch+1)
            running_loss.append(np.average(per_file_total_loss))   
            
               
        print(f'================== epoch is: {epoch} and error is: {np.average(running_loss)}==================\n')
        if (epoch+1) % 2 == 0:
            val_error = run_validation(val_dirs, model, batch_size, epoch, optim)
            experiment.log_metric( name = "Avg Validation loss", value = np.average(val_error), epoch= epoch+1)
        # val_error_at_epoch.append(val_error)
        experiment.log_metric( name = "Avg Training loss", value = np.average(running_loss), epoch= epoch+1)
        


def main():
    train_path = "../recorded-data/train"
    # train_path = "../recorded-data/sandbox"
    train_dirs = [ os.path.join(train_path, dir) for dir in os.listdir(train_path)]
    val_dirs = [ os.path.join('../recorded-data/val', dir) for dir in os.listdir('../recorded-data/val')]
    batch_size = 12
    epochs = 250
    run_training(train_dirs, val_dirs, batch_size, epochs)



main()




scripts/fusion_tester.py
RobotiXX/multimodal-fusion-network
#!/usr/bin/env python
# coding: utf-8
from turtle import pos
import numpy as np
import rospy
import subprocess
import message_filters
import sensor_msgs.point_cloud2 as pc2
import cv2
import sys
import os
import pickle
import torch 

from geometry_msgs.msg import Twist, PoseStamped, Point, Quaternion
from sensor_msgs.msg import PointCloud2, CompressedImage 
from visualization_msgs.msg import Marker
from nav_msgs.msg import Odometry
from pathlib import Path
import matplotlib.pyplot as plt

from scipy.spatial.transform import Rotation as R

from model_builder.pcl.pcl_head import PclMLP
from transformer import ApplyTransformation
from data_builder.gaussian_weights import get_gaussian_weights


device = "cuda:1" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

weights_base = get_gaussian_weights(6,3)[:,:-1] 
# print(weights_base)
weights = np.concatenate([weights_base, weights_base], axis=1)
# weights = torch.tensor(weights)
# weights = weights.to(device)


model = PclMLP()
model.to(device)
ckpt = torch.load('/home/ranjan/Workspace/my_works/fusion-network/scripts/pcl_backbone_changed_model_at_100_0.08454692389459491.pth')
model.load_state_dict(ckpt['model_state_dict'])
model.eval()

content = None
path = '/home/ranjan/Workspace/my_works/fusion-network/recorded-data/val/138258_lg_cwd/snapshot.pickle'

with open(path, 'rb') as data:
    content = pickle.load(data)
v = 'local_goal'


print(f'content loaded: {content[0][v]}')

counter = {'index': 0, 'sub-sampler': 1}
previous_rbt_location = []
previous_velocities = []
play_back_snapshot = {}
image_history = []
pcl_history = []

def get_transformation_matrix(position, quaternion):
    theta = R.from_quat(quaternion).as_euler('XYZ')[2]
    robo_coordinate_in_glob_frame  = np.array([[np.cos(theta), -np.sin(theta), position[0]],
                    [np.sin(theta), np.cos(theta), position[1]],
                    [0, 0, 1]])
    return robo_coordinate_in_glob_frame



def marker_callback(xs, ys):
    marker = Marker()
    marker.header.frame_id='base_link'
    marker.type = Marker.LINE_STRIP
    marker.action = Marker.ADD
    # marker.lifetime = rospy.Duration(1)
    marker.scale.x = 0.02
    marker.color.a = 1.0
    marker.color.r = 1
    marker.color.g = 0.0
    marker.color.b = 1
    marker.pose.orientation = Quaternion(0,0,0,1)

    points_list = []
    points_list.append(Point(y=0,x= 0,z=0))

    for i in range(11):    
        points_list.append(Point(y=ys[i],x= xs[i],z=0))

    marker.points.extend(points_list)
    pub.publish(marker)
    # key = 'index'
    # print(f'published:   {counter[key]}')
    # counter[key] += 1
    # if counter[key] > 10:
    #     counter[key] = 0
    return

def marker_gt(xs, ys):
    marker = Marker()
    marker.header.frame_id='base_link'
    marker.type = Marker.LINE_STRIP
    marker.action = Marker.ADD
    # marker.lifetime = rospy.Duration(1)
    marker.scale.x = 0.02
    marker.color.a = 1.0
    marker.color.r = 0
    marker.color.g = 1.0
    marker.color.b = 0
    marker.pose.orientation = Quaternion(0,0,0,1)

    points_list = []
    points_list.append(Point(y=0,x=0,z=0))

    for i in range(11):    
        points_list.append(Point(y=ys[i],x= xs[i],z=0))

    marker.points.extend(points_list)
    pub_gt.publish(marker)
    # key = 'index'
    # print(f'published:   {counter[key]}')
    # counter[key] += 1
    # if counter[key] > 10:
    #     counter[key] = 0
    return

def get_goals(pts, way_pts):
    goals = pts.detach().cpu().numpy()[0]

    way_pts = way_pts / weights_base
    goals = goals / weights

    

    x = goals[0, :11]
    y = goals[0, 11:]

    print(f'x : {x.shape}')
    print(f'y : {y.shape}')

    wx = way_pts[0,:]
    wy = way_pts[1,:]

    marker_gt(wx,wy)
    marker_callback(x,y)


def get_lidar_points(lidar):
    point_cloud = []
    for point in pc2.read_points(lidar, skip_nans=True, field_names=('x', 'y', 'z')):
        pt_x = point[0]
        pt_y = point[1]
        pt_z = point[2]

        if point[0] >= -1 and point[0] <= 5 and point[1]>=-3 and point[1]<=3 and point[2] >= 0.0299 and point[2] <= 6.0299:
            point_cloud.append([pt_x, pt_y, pt_z])

    # print(len(point_cloud))

    return point_cloud


def read_image(rgb_image):
    np_arr = np.frombuffer(rgb_image.data, np.uint8)
    image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
    return image

def aprrox_sync_callback(lidar, rgb, odom):
    pos = odom.pose.pose.position
    orientation = odom.pose.pose.orientation 
    robot_position = [pos.x, pos.y, pos.z]
    robot_orientation = [orientation.x,orientation.y, orientation.z,orientation.w]

    # print("calling  subsampler")
    if counter['sub-sampler'] % 2 == 0:
        # TODO: these 4 values will be pickled at index counter['index'] except image
        img = read_image(rgb)
        point_cloud = get_lidar_points(lidar)

        print(robot_position)
        print(content[counter['index']]['local_goal'][-1])
        print(content[counter['index']]['robot_position'][0])

        align_content = {
            "pcl": point_cloud,
            "images": [img],
            "local_goal": content[counter['index']]['local_goal'],
            "robot_pos": (robot_position, robot_orientation)
        }

        transformer = ApplyTransformation(align_content)
        # print("transformed")
        pcl, local_goal, way_pts = transformer.__getitem__(0)
        pcl = pcl.to(device)        
        local_goal = local_goal.to(device)
        pcl = pcl.unsqueeze(0)
        local_goal = local_goal.unsqueeze(0)
        with torch.no_grad():
            # print("in")
            pts = model(pcl,local_goal)
            # print("out")
            get_goals(pts, way_pts)
            counter['index'] += 1
            print(counter['index'])            
            counter['sub-sampler'] = 0

    counter['sub-sampler'] += 1



rospy.init_node('listen', anonymous=True)


cmd_publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)

odom = message_filters.Subscriber('/odom', Odometry)
lidar = message_filters.Subscriber('/velodyne_points', PointCloud2)
rgb = message_filters.Subscriber('/image_raw/compressed', CompressedImage)
pub = rospy.Publisher('/world_point', Marker, queue_size=10)
pub_gt = rospy.Publisher('/world_point_gt', Marker, queue_size=10)

ts = message_filters.ApproximateTimeSynchronizer([lidar, rgb, odom], 100, 0.05, allow_headerless=True)
ts.registerCallback(aprrox_sync_callback)

# odom.registerCallback(odom_callback)



rospy.spin()



scripts/global_planner.py
RobotiXX/multimodal-fusion-network
#!/usr/bin/env python
# coding: utf-8
import numpy as np
import rospy
import subprocess
import message_filters
import sensor_msgs.point_cloud2 as pc2
import cv2
import sys
import os
import pickle
import torch 

from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import PointCloud2, CompressedImage
from nav_msgs.msg import Odometry
from pathlib import Path
import matplotlib.pyplot as plt
from transformer import ApplyTransformation
from model_builder.multimodal.fusion_net  import BcFusionModel


counter = {'index': 0, 'sub-sampler': 1}
previous_rbt_location = []
previous_velocities = np.zeros((20,2))
play_back_snapshot = {}
image_history = []
pcl_history = []

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'loading model..')
ckpt = torch.load("/home/ranjan/Workspace/my_works/fusion-network/scripts/model_at_100.pth")
print(f'model loaded')
model = BcFusionModel()
model.load_state_dict(ckpt['model_state_dict'])
model.to(device)
model.eval()
constant_goal = None

# def odom_callback(odom):
#     position = odom.pose.pose.position
#     cmd_vel = odom.twist.twist

#     # print((cmd_vel.linear.x, cmd_vel.angular.z))
#     # store 20 latest liner and angular velocities
#     previous_velocities.insert(0, (cmd_vel.linear.x, cmd_vel.angular.z))
#     if len(previous_velocities) > 20:
#         previous_velocities.pop()

    # return


def get_lidar_points(lidar):
    point_cloud = []
    filtered_points = []
    # print("getting lidar...")
    for point in pc2.read_points(lidar, skip_nans=True, field_names=('x', 'y', 'z')):
        if (point[0]**2 + point[1]**2 + point[2]**2) <= 49:
                filtered_points.append(point)
    # point_cloud.append(filtered_points)
    print(len(filtered_points[:5500]))
    return filtered_points[:5500]


def get_prev_cmd_val():
    return previous_velocities.copy()


def store_image(rgb_image):
    np_arr = np.frombuffer(rgb_image.data, np.uint8)
    image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
    return image
   
def get_filtered_pcl(pcl):
    filtered_data = []
    for i in range(4):
        filtered_data.append(pcl[i][0:5500])
    return filtered_data


def set_lc_goal(goal):
    global constant_goal
    constant_goal = (goal.pose.position.x, goal.pose.position.y)
    print(constant_goal)

def get_goal():
    return constant_goal

def print_image(rgb_images, idx):
    for ind, rgb_image in enumerate(rgb_images):
        # image = cv2.resize(image, (264, 300))
        image_path = os.path.join('./testing', str(idx)+str(ind)+".jpg")
        cv2.imwrite(image_path, rgb_image)

def aprrox_sync_callback(lidar, rgb, odom):
    pos = odom.pose.pose.position
    cmd_vel = odom.twist.twist
    # This function is called at 10Hz
    # Subsampling at each 5th second approx
    if counter['sub-sampler'] % 4 == 0:
        # TODO: these 4 values will be pickled at index counter['index'] except image
        img = store_image(rgb)
        
        image_history.append(img)

       
        
        if len(image_history) > 4:
            image_history.pop(0)
        else:
          msg =  'Ready for inference' if len(image_history) == 4 else 'Not ready yet for inference'
          print(msg) 
        

        point_cloud = get_lidar_points(lidar)
        # print("before append len image hisotry", len(pcl_history))

        pcl_history.append(point_cloud)

        # print("before len image hisotry", len(pcl_history))
        if len(pcl_history) > 4:
            pcl_history.pop(0)

        # print("after len image hisotry", len(pcl_history))
        # prev_cmd_vel.pop()
        
        
        
        if len(image_history) == 4 and constant_goal != None:

            print("inference......")
            
            filtered_pcl = get_filtered_pcl(pcl_history)
            # print(len(prev_cmd_vel))
            # print_image(image_history, counter['index'])
            local_goal = get_goal()
            # print(f'local goal {local_goal}')
            robot_pos = (pos, odom.pose.pose.orientation)
            
            align_content = {
                "pcl": filtered_pcl,
                "images": image_history,
                "prev_cmd_vel": previous_velocities,
                "local_goal": local_goal,
                "robot_pos": robot_pos
            }

            transformer = ApplyTransformation(align_content)
            stacked_images, pcl, lcg, prev_cmd_vel =  transformer.__getitem__(0)            
            
            stacked_images = stacked_images.unsqueeze(0)
            pcl = pcl.unsqueeze(0)
            lcg = lcg.unsqueeze(0)
            prev_cmd_vel= prev_cmd_vel.unsqueeze(0)

            stacked_images = stacked_images.to(device)
            pcl = pcl.to(device)
            lcg= lcg.to(device)
            prev_cmd_vel= prev_cmd_vel.to(device)

            # print(stacked_images.shape)
            # print(pcl.shape)
            # print(lcg.shape)
            # print(prev_cmd_vel.shape)

            with torch.no_grad():
                fsn_lin, fsn_anglr, img_lin, img_anglr, pcl_lin, pcl_anglr = model(stacked_images, pcl, lcg, prev_cmd_vel)
                lin = fsn_lin.detach().cpu().numpy()[0]/100
                anglr = fsn_anglr.detach().cpu().numpy()[0]/5000
                print(f'unscaled: {lin[0]}  , {anglr[0]} \n') 
                # print(anglr[0])
                msg = Twist()
                msg.linear.x = lin[0]
                msg.angular.z = anglr[0]

                cmd_publisher.publish(msg)
                previous_velocities[1:,:] = previous_velocities[0:-1,:]
                previous_velocities[0,0] = lin[0]
                previous_velocities[0,1] = anglr[0]

        
        # previous_rbt_location.append((pos.x, pos.y, counter['index']))
        counter['index'] += 1
        counter['sub-sampler'] = 1

    counter['sub-sampler'] += 1


def lidar_testing(lidar):
    point_cloud = []
    for point in pc2.read_points(lidar, skip_nans=True, field_names=('x', 'y', 'z')):
        pt_x = point[0]
        pt_y = point[1]
        pt_z = point[2]
        # print(pt_x, pt_y, pt_z)
        point_cloud.append([pt_x, pt_y, pt_z])
    
    return point_cloud



rospy.init_node('listen_record_data', anonymous=True)


cmd_publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)

lidar = message_filters.Subscriber('/velodyne_points', PointCloud2)
rgb = message_filters.Subscriber('/zed_node/rgb/image_rect_color/compressed', CompressedImage)
odom = message_filters.Subscriber('zed_node/odom', Odometry)
lc_goal = message_filters.Subscriber('/move_base_simple/goal', PoseStamped)

ts = message_filters.ApproximateTimeSynchronizer([lidar, rgb, odom], 100, 0.05, allow_headerless=True)

ts.registerCallback(aprrox_sync_callback)
# odom.registerCallback(odom_callback)
lc_goal.registerCallback(set_lc_goal)



rospy.spin()



scripts/listner.py
RobotiXX/multimodal-fusion-network
import torch
import os
from tqdm import tqdm
from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from model_builder.image.net import BCModel
from torch.utils.data import DataLoader
import matplotlib.pyplot as plt
import coloredlogs, logging


coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"


def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=True)
    return data_loader

def run_validation(val_files, model, batch_size):
       print("Running Validation..\n")
       val_error = []
       loss = torch.nn.MSELoss()
       with torch.no_grad():
        for val_file in val_files:        
            val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
            for index, (stacked_images, _ ,local_goal, prev_cmd_vel, gt_cmd_vel) in tqdm(enumerate(val_loader)):
                stacked_images = stacked_images.to(device)
                # pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                prev_cmd_vel= prev_cmd_vel.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                pred_cmd_vel = model(stacked_images, local_goal, prev_cmd_vel)
                error = loss(pred_cmd_vel, gt_cmd_vel)
                error_to_number = error.item()
                val_error.append(error_to_number)

        running_loss = sum(val_error)/len(val_error)

        print(f'Average Validation error is:   {running_loss} \n')
        return running_loss
            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = torch.nn.MSELoss()
    model = BCModel().to(device)
    error_at_epoch = []
    val_error_at_epoch = []
    optim = torch.optim.Adam(model.parameters(), lr=0.0001) 
    
    for epoch in range(num_epochs):   
        running_loss = []
        for train_file in train_files:        
            train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )            
            for index, (stacked_images, _ ,local_goal, prev_cmd_vel, gt_cmd_vel) in enumerate(train_loader):
                
                stacked_images = stacked_images.to(device)
                # pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                prev_cmd_vel= prev_cmd_vel.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                # print(f"{gt_cmd_vel.shape = }")

                pred_cmd_vel = model(stacked_images, local_goal, prev_cmd_vel)
                # print(f"{pred_cmd_vel.shape = }")
                error = loss(pred_cmd_vel, gt_cmd_vel)
                optim.zero_grad()
                error.backward()
                optim.step()

                print(f'step is:   {index} and error is:   {error.item()} \n')
                running_loss.append(error.item())

        avg_error_at_epoch = sum(running_loss)/len(running_loss)
        error_at_epoch.append(avg_error_at_epoch)
        print(f'================== epoch is: {epoch} and error is: {error_at_epoch}==================\n')
        val_error_at_epoch.append(run_validation(val_dirs, model, batch_size))

    torch.save(model.state_dict(), "saved_model.pth")
    plt.plot(range(len(error_at_epoch)),error_at_epoch,'-',color='g', label= 'Training')
    plt.plot(range(len(val_error_at_epoch)),val_error_at_epoch,'--',color='r', label='Testing')
    plt.legend(loc='upper left')
    plt.show()



def main():
    train_dirs = [ os.path.join('../recorded-data/train', dir) for dir in os.listdir('../recorded-data/train')]
    val_dirs = [ os.path.join('../recorded-data/val', dir) for dir in os.listdir('../recorded-data/val')]
    batch_size = 16
    epochs = 25
    run_training(train_dirs, val_dirs, batch_size, epochs)



main()


scripts/main.py
RobotiXX/multimodal-fusion-network
"""Different conv architectures to be used as backbone network"""

from pathlib import Path
from typing import List
import torch
import torch.nn as nn
from torchvision import models


def make_mlp(
    dims: List, act: str, l_act: bool = False, bn: bool = True, dropout: float = 0.0
):
    """Create a simple MLP with batch-norm and dropout

    Args:
        dims: (List) a list containing the dimensions of MLP
        act: (str) activation function to be used. Valid activations are [relu, tanh, sigmoid]
        l_act: (bool) whether to use activation after the last linear layer
        bn: (bool) use batch-norm or not. Default is True
        dropout: (float) dropout percentage
    """
    layers = []
    activation = {
        "relu": nn.ReLU(inplace=True),
        "tanh": nn.Tanh(),
        "sigmoid": nn.Sigmoid(),
        "elu": nn.ELU(),
    }[act.lower()]

    for i, (in_dim, out_dim) in enumerate(zip(dims[:-1], dims[1:])):
        layers.append(nn.Linear(in_dim, out_dim, bias=not bn))
        if i != (len(dims) - 2):
            if bn:
                layers.append(nn.BatchNorm1d(out_dim))

            layers.append(activation)

            if dropout > 0.0:
                layers.append(nn.Dropout(p=dropout))

    if l_act:
        layers.append(activation)

    return nn.Sequential(*layers)


def get_backbone(
    arch: str = "resnet18",
    n_frames: int = 4,
    n_channels: int = 3,
):
    """Retrieve backbone model"""
    if "resnet" in arch:
        return _get_resnet(n_frames, arch, n_channels)
    elif "mobilenet" in arch:
        return _get_mobilenet(n_frames, arch, n_channels)


def _get_resnet(
    n_frames: int = 4,
    arch: str = "resnet18",
    n_channels: int = 3,
):
    """Get different versions of ResNet and change input/output dimensions"""
    weights = {
        "resnet18": models.ResNet18_Weights.DEFAULT,
        "resnet34": models.ResNet34_Weights.DEFAULT,
        "resnet50": models.ResNet50_Weights.DEFAULT,
    }.get(arch.lower(), models.ResNet18_Weights.DEFAULT)

    model = {
        "resnet18": models.resnet18,
        "resnet34": models.resnet34,
        "resnet50": models.resnet50,
    }.get(arch.lower(), models.resnet18)()

    model.conv1 = nn.Conv2d(
        in_channels=n_frames * n_channels, out_channels=model.conv1.out_channels, kernel_size=model.conv1.kernel_size
    )

    model.fc = nn.Identity()

    model.layer4 = nn.Identity()
    model.avgpool = nn.Identity()

    custom_model = nn.Sequential(
        *list(model.children())[:-3]
    )

    # print(custom_model)
    return custom_model


def _get_mobilenet(
    n_frames: int = 4,
    arch: str = "resnet18",
    pretrained: bool = False,
    n_channels: int = 3,
):
    """Get different versions of mobilenet and change input/output dimensions
    Requires torchvision version 0.9.1
    """
    model = {
        "mobilenet_v3_small": models.mobilenet_v3_small,
        "mobilenet_v3_large": models.mobilenet_v3_large,
        "mobilenet_v2": models.mobilenet_v2,
    }.get(arch.lower(), models.mobilenet_v3_small)(pretrained=pretrained)

    model.features[0][0] = nn.Conv2d(
        in_channels=n_frames * n_channels,
        out_channels=model.features[0][0].out_channels,

    )

    if "v2" in arch.lower():
        model.classifier = nn.Linear(model.classifier[1].in_features, 512)
    else:
        model.classifier[3] = nn.Linear(model.classifier[3].in_features, 512)

    return model


def get_unet(
    model_dir: str,
    inter_repr: bool = True,
    n_frames: int = 4,
    gamma: int = 2,
    b: int = 1,
    n_channels: int = 3,
):
    """Get UNet"""
    model = UNet(gamma=gamma, b=b, inter_repr=inter_repr)
    state_dict = torch.load(Path(model_dir).resolve())
    model.load_state_dict(state_dict, strict=False)

    entry_block = EfficientConvBlock(
        in_channels=n_frames * n_channels, out_ch=3, gamma=gamma, b=b
    )

    return nn.Sequential(entry_block, model)


if __name__ == "__main__":
    # print(_get_resnet(arch='resnet18', pretrained=True))
    # print(_get_resnet(arch='resnet18')(torch.rand(1, 12, 256, 256)).shape)
    # print(_get_mobilenet(arch='mobilenet_v2', pretrained=False))
    # print(_get_mobilenet(arch='mobilenet_v2')(torch.rand(1, 12, 256, 256)).shape)
    # model = make_mlp([512, 512, 512,1], act='relu', bn=True, l_act=True)
    # print(model)
    pass

scripts/model_builder/image/backbone.py
RobotiXX/multimodal-fusion-network
from typing import List

import torch
import torch.nn as nn
from torchvision.models import resnet18, ResNet18_Weights

from .backbone import get_backbone
from collections import OrderedDict


class ImageFusionModel(nn.Module):
    def __init__(
        self,
        output_layers = [5,6,7],
        backbone: str = "resnet18",
        n_frames: int = 1,
        n_channels: int = 3,
        act: str = 'relu',
        l_act: bool = False, bn: bool = False, dropout: float = 0.0,

    ):

        super().__init__()
        self.output_layers = output_layers
        # print(backbone,n_frames,n_channels)
        self.backbone = get_backbone(backbone, n_frames, n_channels)
    #     print(self.backbone)
        self.max_pool = nn.MaxPool2d(kernel_size= 5, stride=2, padding=(2,2))
    #     self.selected_out = OrderedDict()
    #     self.fhooks = []

    #     for i, l in enumerate(list(self.backbone._modules.keys())):
    #         if i in self.output_layers:
    #             self.fhooks.append(getattr(self.backbone,l).register_forward_hook(self.forward_hook(l)))
            

    # def forward_hook(self, layer_name):
    #     def hook(module, input, output):
    #         self.selected_out[layer_name] = output
    #     return hook
    
    def forward(self, stacked_images):

        batchsize = stacked_images.size()[0]
        # print(f"{stacked_images.shape}")
        # image features in shape (B, 512)
        imgs = self.max_pool( self.backbone(stacked_images))
        

        linear_img_feat = imgs.contiguous().view(batchsize, -1)        

        # print(linear_img_feat.shape)
        # print(f'{pooled_features_linear.shape}')

        return linear_img_feat


# ImageFusionModel()
scripts/model_builder/image/backbone_fusion.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
from .backbone_fusion import ImageFusionModel
from .backbone import make_mlp
from .tf_img import CustomTransformerModelImage 



class ImageHeadMLP(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()
        self.backbone = ImageFusionModel()        
        self.img_transformer = CustomTransformerModelImage()

        self.goal_encoder = make_mlp( [2, 128, 64], 'relu', False, False, 0.0)

        self.lstm_input = 50176+64
        self.hidden_state_dim = 512
        self.num_layers = 5

        self.rnn = nn.RNN(self.lstm_input, self.hidden_state_dim, self.num_layers, nonlinearity='relu',batch_first=True)

        self.after_rnn = nn.Sequential(
            nn.Linear(512,256),            
            nn.ELU() 
        )

        self.predict_path = nn.Linear(256,8)

        self.prediction_encoder = nn.Sequential(
            nn.Linear(8, 256),
            nn.ELU(),
            nn.Linear(256,128),
            nn.ELU()
        )

        self.predict_vel = nn.Sequential(
            nn.Linear(512+128, 512),
            nn.ELU(),
            nn.Linear(512,2)
        )

    def forward(self, input, goal):
        
        h0 = torch.zeros(self.num_layers, 1, self.hidden_state_dim, device='cuda')
        
        image_features = self.backbone(input)                
        goal = self.goal_encoder(goal)

        img_feat_with_goal = torch.cat([image_features, goal],dim=-1)
        img_feat_with_goal = img_feat_with_goal.unsqueeze(0)

        rnn_out, _ = self.rnn(img_feat_with_goal, h0)
        
        rnn_out = rnn_out.squeeze(0)
       
        final_feat = self.after_rnn(rnn_out)
        
        prediction_path = self.predict_path(final_feat)

        encoded_prediction = self.prediction_encoder(prediction_path)

        tf_input = torch.cat([rnn_out, encoded_prediction],dim=-1).unsqueeze(0)
        
        tf_out = self.img_transformer(tf_input)

        tf_out = tf_out.squeeze(0)

        predicted_vel = self.predict_vel(tf_out)
          
        return rnn_out, prediction_path, predicted_vel





scripts/model_builder/image/image_head.py
RobotiXX/multimodal-fusion-network
from typing import List

import torch
import torch.nn as nn
from torchvision.models import resnet18, ResNet18_Weights

from .backbone import get_backbone, make_mlp


class BCModel(nn.Module):
    def __init__(
        self,
        backbone: str = "resnet18",
        controller_encoder: list = [768, 64, 2],
        goal_encoder: list = [2, 256, 128],
        prev_cmd_encoder: list = [2 * 20, 64, 128],
        n_frames: int = 4,
        n_channels: int = 3,
        act: str = 'relu',
        l_act: bool = False, bn: bool = False, dropout: float = 0.0
    ):

        super().__init__()
        self.backbone = get_backbone(backbone, n_frames, n_channels)
        self.controller = make_mlp(controller_encoder, act, l_act, bn, dropout)
        self.goal_encoder = make_mlp(goal_encoder, act, l_act, bn, dropout)
        self.prev_cmd_encoder = make_mlp(prev_cmd_encoder, act, l_act, bn, dropout)

    def forward(self, stacked_images, local_goal, prev_cmd_vel):
        # print(f"{img.stacked_images = }")
        # image features in shape (B, 512)
        imgs = self.backbone(stacked_images)
        # goal features in shape (B, 128)
        goal = self.goal_encoder(local_goal)
        # prec_cmd_vel features in shape (B, 128)
        prev_cmd = self.prev_cmd_encoder(prev_cmd_vel)
        # concat all encoded features together along the last dim,
        # making a tensor of shape (B, 512 + 128 + 128) = (B, 768)
        features = torch.cat([imgs, goal, prev_cmd], dim=-1)
        # return the action in shape (B, 2)
        return self.controller(features)

scripts/model_builder/image/net.py
RobotiXX/multimodal-fusion-network
import torch
import torch.nn as nn

class CustomTransformerModelImage(nn.Module):
    def __init__(self, hidden_dim = 512+128, num_layers = 2, nhead=4):
        super(CustomTransformerModelImage, self).__init__()                
        self.transformer_encoder_layer = nn.TransformerEncoderLayer(d_model=hidden_dim, nhead=nhead, batch_first=True)
        self.transformer_encoder = nn.TransformerEncoder(self.transformer_encoder_layer, num_layers=num_layers)
    

    def forward(self, x):
        # linear_output = self.linear(x)
        transformer_output = self.transformer_encoder(x)
        # output = self.fc(transformer_output)
        return transformer_output

scripts/model_builder/image/tf_img.py
RobotiXX/multimodal-fusion-network
import torch
import torch.nn as nn




class CustomActivation(nn.Module):
    def __init__(
            self,
    ):
        super().__init__()
        self.slope = torch.tensor(0.001)

    def forward(self, input):
        return torch.multiply(self.slope, input)
scripts/model_builder/multimodal/custom_activation.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
from ..image.backbone import make_mlp
from ..image.backbone_fusion import ImageFusionModel
from ..pcl.pointnet_fusion import PointNetDenseFusionModel
from ..pcl.pointnet_backbone import PclBackbone
from .fustion_mlp import FusionMLP
from .pcl_mlp import PclMLP
from .image_mlp import ImageMLP


class BcFusionModel(nn.Module):
    def __init__(
        self,        
        backbone_img: str = "resnet18",
        sparse_shape = [480, 360, 32],
        controller_encoder: list = [(198256+512), 128, 64, 32, 2],
        goal_encoder: list = [4, 256, 128],
        prev_cmd_encoder: list = [2 * 20, 64, 128],
        n_frames: int = 4,
        n_channels: int = 3,
        act: str = 'relu',
        l_act: bool = False, bn: bool = True, dropout: float = 0.0
    ):

        super().__init__()
        
        self.backbone_pcl = PclBackbone()
        self.backbone_img = ImageFusionModel(backbone=backbone_img, n_frames= n_frames, n_channels=n_channels)
        self.goal_encoder = make_mlp(goal_encoder, act, l_act, False, dropout)
        # self.prev_cmd_encoder = make_mlp(prev_cmd_encoder, act, l_act, False, dropout)

        self.mx_pool_lyr2_img = nn.MaxPool2d(kernel_size= 4, stride=3)
        self.mx_pool_lyr3_img = nn.MaxPool2d(kernel_size= 2, stride=3)
        self.mx_pool_lyr4_img = nn.MaxPool2d(kernel_size= 2, stride=2)

        self.mx_pool_lyr1_pcl = nn.MaxPool1d(kernel_size= 12, stride=  8)
        self.mx_pool_lyr2_pcl = nn.MaxPool1d(kernel_size= 8, stride= 6)
        self.mx_pool_lyr3_pcl = nn.MaxPool1d(kernel_size= 2, stride= 2)

        self.controller_img = ImageMLP()
        self.controller_pcl = PclMLP()
        self.fusion_mlp = FusionMLP()
        

    def forward(self, stacked_images, pcl, local_goal, prev_cmd_vel):


        imgs_feat, intr_img_rep = self.backbone_img(stacked_images)
        point_cloud_feat, intr_pts_rep = self.backbone_pcl(pcl.float())
        
        goal = self.goal_encoder(local_goal)
        # prev_cmd = self.prev_cmd_encoder(prev_cmd_vel)

        pooled_lyr1_img = self.mx_pool_lyr2_img( intr_img_rep['layer2'] )
        pooled_lyr2_img = self.mx_pool_lyr3_img( intr_img_rep['layer3'] )
        pooled_lyr3_img = self.mx_pool_lyr4_img( intr_img_rep['layer4'] )

        pooled_lyr1_img = pooled_lyr1_img.contiguous().view(pooled_lyr1_img.shape[0], -1)
        pooled_lyr2_img = pooled_lyr2_img.contiguous().view(pooled_lyr2_img.shape[0], -1)
        pooled_lyr3_img = pooled_lyr3_img.contiguous().view(pooled_lyr3_img.shape[0], -1)
        

        pooled_lyr1_pcl = intr_pts_rep[0]
        pooled_lyr2_pcl = intr_pts_rep[1]
        pooled_lyr3_pcl = point_cloud_feat

        # print(f'pcl:{pooled_lyr1_pcl.shape}, image:{pooled_lyr1_img.shape}')
        # print(f'pcl:{pooled_lyr2_pcl.shape}, image:{pooled_lyr2_img.shape}')
        # print(f'pcl:{pooled_lyr3_pcl.shape}, image:{pooled_lyr3_img.shape}')

        fusion_input1 = torch.cat([pooled_lyr1_img, pooled_lyr1_pcl], dim=1)
        fusion_input2 = torch.cat([pooled_lyr2_img, pooled_lyr2_pcl], dim=1)
        fusion_input3 = torch.cat([pooled_lyr3_img, pooled_lyr3_pcl], dim=1)



        predict_fusion_model_lin, predict_fusion_model_angular = self.fusion_mlp(fusion_input1, fusion_input2, fusion_input3, goal)

        input_image_features = torch.cat([imgs_feat, goal],dim=-1)
        img_lin, img_angular = self.controller_img(input_image_features)

        input_pcl_features = torch.cat([pooled_lyr1_pcl, goal],dim=-1)
        pcl_lin, pcl_angular = self.controller_pcl(input_pcl_features)


        return predict_fusion_model_lin, predict_fusion_model_angular, img_lin, img_angular, pcl_lin, pcl_angular

scripts/model_builder/multimodal/fusion_net.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn




class FusionMLP(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()


        self.linear1 = nn.Linear( +41472, 512)
        self.linear2 = nn.Linear(64000+20736+512,256)
        self.linear3 = nn.Linear(32000+25088+256,256)
        self.linear4 = nn.Linear(3*128, 512)
        self.linear5 = nn.Linear(512,1)

        self.angular = nn.Sequential(
            nn.Linear(512,64),
            nn.ReLU(),
            nn.Linear(64,1)
        )
        

        self.act1 = nn.LeakyReLU()
        self.act2 = nn.LeakyReLU()
        self.act3 = nn.LeakyReLU()
        self.act4 = nn.ReLU()


    def forward(self, input_l1, input_l2, input_l3, goal):
        
        x = self.linear1(input_l1)
        x = self.act1(x)        
       

        x = torch.cat([input_l2,x], dim=-1)
        x = self.linear2(x)
        x = self.act2(x)          

        x = torch.cat([input_l3,x], dim=-1)
        x = self.linear3(x)
        x = self.act3(x)   
      
        # print(x.shape,goal.shape, prev_cmd_vel.shape)
        x = torch.cat([x, goal], dim=-1)
        x = self.linear4(x)
        x_shared = self.act4(x)  

        x = self.linear5(x_shared)

        y = self.angular(x_shared)
          
        return x, y





scripts/model_builder/multimodal/fustion_mlp.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn




class ImageMLP(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()

        self.common = nn.Sequential(
            nn.Linear(512+128,128),
            # nn.BatchNorm1d(128),
            nn.LeakyReLU(),
            nn.Linear(128,64),
            # nn.BatchNorm1d(64),
            nn.ReLU(),
            nn.Linear(64,32),
            # nn.BatchNorm1d(32),
            nn.ReLU()
        )

        self.linear_vel = nn.Linear(32,1)
        self.angular_vel = nn.Linear(32,1)

    def forward(self, input):
        
        feat_shared = self.common(input)

        x = self.linear_vel(feat_shared)
        y = self.angular_vel(feat_shared)
          
        return x, y





scripts/model_builder/multimodal/image_mlp.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
from ..pcl.pcl_head import PclMLP
from ..image.image_head import ImageHeadMLP
from .tf_model import CustomTransformerModel
from ..image.backbone import make_mlp

def set_trainable_false(model):
    for param in model.parameters():
        param.requires_grad = False    

def torch_load_weights(path):
    check_point = torch.load(path)
    model_weights = check_point['model_state_dict']
    return model_weights

class MultiModalNet(nn.Module):
    def __init__(self):

        super().__init__()
        
        self.image =  ImageHeadMLP()        
        self.pcl =  PclMLP()
        self.transformer = CustomTransformerModel()


        self.modality_fusion_layer = nn.Sequential(
            nn.Linear(1024+512,2304),
            nn.ELU(),
            nn.Linear(2304,1024),
            nn.ELU()
        )

        self.goal_encoder = make_mlp( [2, 128, 64], 'relu', False, False, 0.0)

        self.global_path_predictor = nn.Sequential(
            nn.Linear(1024+64, 512),
            nn.ELU(),
            nn.Linear(512,8),            
        )

        self.global_path_encoder = nn.Sequential(
            nn.Linear(8, 256),
            nn.ELU(),
            nn.Linear(256,128),
            nn.ELU()
        )

        self.joint_perception_path_feautres = nn.Sequential(
            nn.Linear(128+1024,512),
            nn.ELU()
        )

        self.predict_vel = nn.Linear(512,2)        

    def forward(self, stacked_images, pcl, local_goal):
        

        rnn_img_out, img_path, img_vel = self.image(stacked_images, local_goal)
        rnn_pcl_out, pcl_path, pcl_vel = self.pcl(pcl, local_goal)

        backbone_feats = torch.cat([rnn_pcl_out, rnn_img_out], dim=-1)
        fustion_features = self.modality_fusion_layer(backbone_feats)        
        
        encoded_goal = self.goal_encoder(local_goal)

        fsn_global_path_feats = torch.cat([fustion_features, encoded_goal], dim=-1)
        fsn_global_path = self.global_path_predictor(fsn_global_path_feats)

        encoded_global_path = self.global_path_encoder(fsn_global_path)

        # second_layer_features = torch.cat([final_pcl_feat,final_img_feat], dim=-1)
        # global_path_encoding = self.global_path_fusion(second_layer_features)
        

        final_features_concat = torch.cat([fustion_features, encoded_global_path], dim=-1).unsqueeze(0)
        
        final_feat = self.transformer(final_features_concat)        

        final_feat = final_feat.squeeze(0)
        
        fustion_perception_path = self.joint_perception_path_feautres(final_feat)

        fusion_vel = self.predict_vel(fustion_perception_path)

        return fsn_global_path, fusion_vel,  img_path, img_vel, pcl_path, pcl_vel

scripts/model_builder/multimodal/multi_net.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
from ..image.backbone import make_mlp, get_backbone
from ..pcl.pointnet import PointNetDenseCls



class BCModelPcl(nn.Module):
    def __init__(
        self,        
        backbone_img: str = "resnet18",
        sparse_shape = [480, 360, 32],
        controller_encoder: list = [(198256+512), 128, 64, 32, 2],
        goal_encoder: list = [2, 256, 128],
        prev_cmd_encoder: list = [2 * 20, 64, 128],
        n_frames: int = 4,
        n_channels: int = 3,
        act: str = 'relu',
        l_act: bool = False, bn: bool = False, dropout: float = 0.0
    ):

        super().__init__()
        self.backbone_pcl = PointNetDenseCls().float()
        self.backbone_img = get_backbone(backbone_img, n_frames, n_channels)
        self.controller = make_mlp(controller_encoder, act, l_act, bn, dropout)
        self.goal_encoder = make_mlp(goal_encoder, act, l_act, bn, dropout)
        self.prev_cmd_encoder = make_mlp(prev_cmd_encoder, act, l_act, bn, dropout)

    def forward(self, pcl, stacked_images,local_goal, prev_cmd_vel):
        # print(f"{img.stacked_images = }")
        # image features in shape (B, 512)
        imgs_feat = self.backbone_img(stacked_images)
        point_cloud_feat = self.backbone_pcl(pcl.float())
        # goal features in shape (B, 128)
        goal = self.goal_encoder(local_goal)
        # prec_cmd_vel features in shape (B, 128)
        # print(prev_cmd_vel.shape)
        prev_cmd = self.prev_cmd_encoder(prev_cmd_vel)
        # concat all encoded features together along the last dim,
        # making a tensor of shape (B, 512 + 128 + 128) = (B, 768)
        # print(point_cloud.shape)
        # print(goal.shape)
        # print(prev_cmd.shape)
        multimodal_feat = torch.cat([point_cloud_feat,imgs_feat], dim=1)
        features = torch.cat([multimodal_feat, goal, prev_cmd], dim=-1)
        # return the action in shape (B, 2)
        return self.controller(features)

scripts/model_builder/multimodal/net.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn




class PclMLP(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()

        self.common = nn.Sequential(
            nn.Linear(221184+128,128),
            # nn.BatchNorm1d(128),
            nn.LeakyReLU(),
            nn.Linear(128,64),
            # nn.BatchNorm1d(64),
            nn.ReLU(),
            nn.Linear(64,32),
            # nn.BatchNorm1d(32),
            nn.ReLU()
        )

        self.linear_vel = nn.Linear(32,1)
        self.angular_vel = nn.Linear(32,1)

    def forward(self, input):
        
        feat_shared = self.common(input)

        x = self.linear_vel(feat_shared)
        y = self.angular_vel(feat_shared)
          
        return x, y





scripts/model_builder/multimodal/pcl_mlp.py
RobotiXX/multimodal-fusion-network
import torch
import torch.nn as nn

class CustomTransformerModel(nn.Module):
    def __init__(self, hidden_dim = 128+1024, num_layers = 2, nhead=8):
        super(CustomTransformerModel, self).__init__()                
        self.transformer_encoder_layer = nn.TransformerEncoderLayer(d_model=hidden_dim, nhead=nhead, batch_first=True)
        self.transformer_encoder = nn.TransformerEncoder(self.transformer_encoder_layer, num_layers=num_layers)
    

    def forward(self, x):
        # linear_output = self.linear(x)
        transformer_output = self.transformer_encoder(x)
        # output = self.fc(transformer_output)
        return transformer_output

scripts/model_builder/multimodal/tf_model.py
RobotiXX/multimodal-fusion-network
import torch

class Backbone(torch.nn.Module):
    def __init__(self,
                 cylin_model,
                 segmentator_spconv,
                 sparse_shape,
                 ):
        super().__init__()
        self.name = "cylinder_asym"

        self.cylinder_3d_generator = cylin_model

        self.cylinder_3d_spconv_seg = segmentator_spconv

        self.sparse_shape = sparse_shape

    def forward(self, train_pt_fea_ten, train_vox_ten, batch_size):
        coords, features_3d = self.cylinder_3d_generator(train_pt_fea_ten, train_vox_ten)

        spatial_features = self.cylinder_3d_spconv_seg(features_3d, coords, batch_size)

        return spatial_features
scripts/model_builder/pcl/backbone.py
RobotiXX/multimodal-fusion-network
import torch
import torch.nn as nn
import torch.nn.functional as F
from torch_scatter import scatter_max


class FeatureEncoder(nn.Module):
    def __init__(
            self , 
            grid_size = [480, 360, 32], 
            output_feat_dim = 256, 
            feat_dim = 9
        ):        
        super(FeatureEncoder, self).__init__()

        self.feat_encoder = nn.Sequential(
            nn.BatchNorm1d(feat_dim),

            nn.Linear(feat_dim, 64),
            nn.BatchNorm1d(64),
            nn.ReLU(),

            nn.Linear(64, 128),
            nn.BatchNorm1d(128),
            nn.ReLU(),

            nn.Linear(128, 256),
            nn.BatchNorm1d(256),
            nn.ReLU(),

            nn.Linear(256, output_feat_dim)
        )

    def forward(self, point_feat, point_index):
        device = point_feat[0].get_device()
        
        # concat everything
        cat_point_index = []
        
        for batch_i in  range(len(point_index)):
            cat_point_index.append(F.pad(point_index[batch_i], (1,0), mode='constant', value=batch_i))
        
        point_feat = torch.cat(point_feat, dim=0)
        cat_point_index = torch.cat(cat_point_index, dim=0)        

        # shuffle data
        num_points = cat_point_index.shape[0]
        
        shuffled_ind = torch.randperm(num_points, device= device)
        point_feat = point_feat[shuffled_ind, :]
        cat_point_index = cat_point_index[shuffled_ind, :]

        # unique xy grid index
        unq_index, unq_inv, unq_cnt = torch.unique(cat_point_index, return_inverse=True, return_counts=True, dim=0)
        unq_index = unq_index.type(torch.int64)

        # process feature
        processed_cat_pt_fea = self.PPmodel(point_feat)
        pooled_data = torch_scatter.scatter_max(processed_cat_pt_fea, unq_inv, dim=0)[0]

        return unq_index, pooled_data
scripts/model_builder/pcl/feature_encoder.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
# from torchvision.models import resnet18, ResNet18_Weights
from .backbone import Backbone
from ..image.backbone import make_mlp
# from .feature_encoder import FeatureEncoder
# from .spconv import Asymm_3d_spconv
from .pointnet import PointNetDenseCls



class BCModelPcl(nn.Module):
    def __init__(
        self,        
        sparse_shape = [480, 360, 32],
        controller_encoder: list = [216256, 256, 128, 64, 2],
        goal_encoder: list = [2, 256, 128],
        prev_cmd_encoder: list = [2 * 18, 64, 128],
        n_frames: int = 4,
        n_channels: int = 3,
        act: str = 'relu',
        l_act: bool = False, bn: bool = False, dropout: float = 0.0
    ):

        super().__init__()
        self.backbone = PointNetDenseCls().float()
        self.controller = make_mlp(controller_encoder, act, l_act, bn, dropout)
        self.goal_encoder = make_mlp(goal_encoder, act, l_act, bn, dropout)
        self.prev_cmd_encoder = make_mlp(prev_cmd_encoder, act, l_act, bn, dropout)

    def forward(self, pcl, local_goal, prev_cmd_vel):
        # print(f"{img.stacked_images = }")
        # image features in shape (B, 512)
        point_cloud = self.backbone(pcl.float())
        # goal features in shape (B, 128)
        goal = self.goal_encoder(local_goal)
        # prec_cmd_vel features in shape (B, 128)
        prev_cmd = self.prev_cmd_encoder(prev_cmd_vel)
        # concat all encoded features together along the last dim,
        # making a tensor of shape (B, 512 + 128 + 128) = (B, 768)
        print(point_cloud.shape)
        print(goal.shape)
        print(prev_cmd.shape)
        features = torch.cat([point_cloud, goal, prev_cmd], dim=-1)
        # return the action in shape (B, 2)
        return self.controller(features)

scripts/model_builder/pcl/net.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn
from ..image.backbone import make_mlp
from .pointnet_backbone import PclBackbone
from .tf_pcl import CustomTransformerModelPcl



class PclMLP(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()

        self.backbone_pcl = PclBackbone().float()
        self.pcl_transformer = CustomTransformerModelPcl()
        self.goal_encoder = make_mlp( [2, 128, 64], 'relu', False, False, 0.0)

        self.lstm_input = 158400+64
        self.hidden_state_dim = 1024
        self.num_layers = 4

        self.rnn = nn.RNN(self.lstm_input, self.hidden_state_dim, self.num_layers, nonlinearity='relu',batch_first=True)

        self.after_rnn = nn.Sequential(
            nn.Linear(1024,512),
            nn.ELU()                              
        )        

        self.predict_path = nn.Linear(512,8)            

        self.predict_path_encoder = nn.Sequential(
            nn.Linear(8, 256),
            nn.ELU(),
            nn.Linear(256,128),
            nn.ELU()                            
        )        

        self.predict_vel = nn.Sequential(
            nn.Linear(1024+128, 512),
            nn.ELU(),
            nn.Linear(512,2)
        )
        
                

    def forward(self, input, goal):

        h0 = torch.zeros(self.num_layers, 1, self.hidden_state_dim, device='cuda')
        # c0 = torch.zeros(self.num_layers, 1, self.hidden_state_dim,device='cuda')

        point_cloud_feat = self.backbone_pcl(input.float())        
        goal = self.goal_encoder(goal)            
        
        pcl_goal_concat = torch.cat([point_cloud_feat, goal],dim=-1)
        
        pcl_goal_concat = pcl_goal_concat.unsqueeze(0)

        rnn_out, _ = self.rnn(pcl_goal_concat, h0)

        rnn_out = rnn_out.squeeze(0)

        final_feat = self.after_rnn(rnn_out)

        prediction_path = self.predict_path(final_feat)                

        encoded_path = self.predict_path_encoder(prediction_path)

        tf_input = torch.cat([rnn_out, encoded_path],dim=-1).unsqueeze(0)
        
        tf_out = self.pcl_transformer(tf_input)

        tf_out = tf_out.squeeze(0)

        predicted_vel = self.predict_vel(tf_out)

        return rnn_out, prediction_path, predicted_vel




scripts/model_builder/pcl/pcl_head.py
RobotiXX/multimodal-fusion-network
from __future__ import print_function
import torch
import torch.nn as nn
import torch.nn.parallel
import torch.utils.data
from torch.autograd import Variable
import numpy as np
import torch.nn.functional as F


class STN3d(nn.Module):
    def __init__(self):
        super(STN3d, self).__init__()
        self.conv1 = torch.nn.Conv1d(3, 64, 1)
        self.conv2 = torch.nn.Conv1d(64, 128, 1)
        self.conv3 = torch.nn.Conv1d(128, 1024, 1)
        self.fc1 = nn.Linear(1024, 512)
        self.fc2 = nn.Linear(512, 256)
        self.fc3 = nn.Linear(256, 9)
        self.relu = nn.ReLU()

        self.bn1 = nn.BatchNorm1d(64)
        self.bn2 = nn.BatchNorm1d(128)
        self.bn3 = nn.BatchNorm1d(1024)
        self.bn4 = nn.BatchNorm1d(512)
        self.bn5 = nn.BatchNorm1d(256)


    def forward(self, x):
        batchsize = x.size()[0]
        x = F.relu(self.bn1(self.conv1(x)))
        x = F.relu(self.bn2(self.conv2(x)))
        x = F.relu(self.bn3(self.conv3(x)))
        x = torch.max(x, 2, keepdim=True)[0]
        x = x.view(-1, 1024)

        x = F.relu(self.fc1(x))
        x = F.relu(self.fc2(x))
        x = self.fc3(x)

        iden = Variable(torch.from_numpy(np.array([1,0,0,0,1,0,0,0,1]).astype(np.float32))).view(1,9).repeat(batchsize,1)
        if x.is_cuda:
            iden = iden.cuda()
        x = x + iden
        x = x.view(-1, 3, 3)
        return x


class STNkd(nn.Module):
    def __init__(self, k=64):
        super(STNkd, self).__init__()
        self.conv1 = torch.nn.Conv1d(k, 64, 1)
        self.conv2 = torch.nn.Conv1d(64, 128, 1)
        self.conv3 = torch.nn.Conv1d(128, 1024, 1)
        self.fc1 = nn.Linear(1024, 512)
        self.fc2 = nn.Linear(512, 256)
        self.fc3 = nn.Linear(256, k*k)
        self.relu = nn.ReLU()

        self.bn1 = nn.BatchNorm1d(64)
        self.bn2 = nn.BatchNorm1d(128)
        self.bn3 = nn.BatchNorm1d(1024)
        self.bn4 = nn.BatchNorm1d(512)
        self.bn5 = nn.BatchNorm1d(256)

        self.k = k

    def forward(self, x):
        batchsize = x.size()[0]
        x = F.relu(self.bn1(self.conv1(x)))
        x = F.relu(self.bn2(self.conv2(x)))
        x = F.relu(self.bn3(self.conv3(x)))
        x = torch.max(x, 2, keepdim=True)[0]
        x = x.view(-1, 1024)

        x = F.relu(self.bn4(self.fc1(x)))
        x = F.relu(self.bn5(self.fc2(x)))
        x = self.fc3(x)

        iden = Variable(torch.from_numpy(np.eye(self.k).flatten().astype(np.float32))).view(1,self.k*self.k).repeat(batchsize,1)
        if x.is_cuda:
            iden = iden.cuda()
        x = x + iden
        x = x.view(-1, self.k, self.k)
        return x

class PointNetfeat(nn.Module):
    def __init__(self, global_feat = True, feature_transform = False):
        super(PointNetfeat, self).__init__()
        self.stn = STN3d()
        self.conv1 = torch.nn.Conv1d(3, 64, 1)
        self.conv2 = torch.nn.Conv1d(64, 128, 1)
        self.conv3 = torch.nn.Conv1d(128, 1024, 1)
        self.bn1 = nn.BatchNorm1d(64)
        self.bn2 = nn.BatchNorm1d(128)
        self.bn3 = nn.BatchNorm1d(1024)
        self.global_feat = global_feat
        self.feature_transform = feature_transform
        if self.feature_transform:
            self.fstn = STNkd(k=64)

    def forward(self, x):
        n_pts = x.size()[2]
        trans = self.stn(x)
        x = x.transpose(2, 1)
        x = torch.bmm(x, trans)
        x = x.transpose(2, 1)
        x = F.relu(self.bn1(self.conv1(x)))

        if self.feature_transform:
            trans_feat = self.fstn(x)
            x = x.transpose(2,1)
            x = torch.bmm(x, trans_feat)
            x = x.transpose(2,1)
        else:
            trans_feat = None

        pointfeat = x
        x = F.relu(self.bn2(self.conv2(x)))
        x = self.bn3(self.conv3(x))
        x = torch.max(x, 2, keepdim=True)[0]
        x = x.view(-1, 1024)
        if self.global_feat:
            return x, trans, trans_feat
        else:
            x = x.view(-1, 1024, 1).repeat(1, 1, n_pts)
            return torch.cat([x, pointfeat], 1), trans, trans_feat

class PointNetCls(nn.Module):
    def __init__(self, k=2, feature_transform=False):
        super(PointNetCls, self).__init__()
        self.feature_transform = feature_transform
        self.feat = PointNetfeat(global_feat=True, feature_transform=feature_transform)
        self.fc1 = nn.Linear(1024, 512)
        self.fc2 = nn.Linear(512, 256)
        self.fc3 = nn.Linear(256, k)
        self.dropout = nn.Dropout(p=0.3)
        self.bn1 = nn.BatchNorm1d(512)
        self.bn2 = nn.BatchNorm1d(256)
        self.relu = nn.ReLU()

    def forward(self, x):
        x, trans, trans_feat = self.feat(x)
        x = F.relu(self.bn1(self.fc1(x)))
        x = F.relu(self.bn2(self.dropout(self.fc2(x))))
        x = self.fc3(x)
        return F.log_softmax(x, dim=1), trans, trans_feat


class PointNetDenseCls(nn.Module):
    def __init__(self, k = 64, feature_transform=False):
        super(PointNetDenseCls, self).__init__()
        self.k = k
        self.feature_transform=feature_transform
        self.feat = PointNetfeat(global_feat=False, feature_transform=feature_transform)
        self.conv1 = torch.nn.Conv1d(1088, 512, 1)
        self.conv2 = torch.nn.Conv1d(512, 256, 1)
        self.conv3 = torch.nn.Conv1d(256, 128, 1)
        self.conv4 = torch.nn.Conv1d(128, 9, 1)
        self.bn1 = nn.BatchNorm1d(512)
        self.bn2 = nn.BatchNorm1d(256)
        self.bn3 = nn.BatchNorm1d(128)

    def forward(self, x):
        batchsize = x.size()[0]
        n_pts = x.size()[2]
        x, trans, trans_feat = self.feat(x)
        x = F.relu(self.bn1(self.conv1(x)))
        x = F.relu(self.bn2(self.conv2(x)))
        x = F.relu(self.bn3(self.conv3(x)))
        x = self.conv4(x)
        # x = x.transpose(2,1).contiguous()
        # x = F.log_softmax(x.view(-1,self.k), dim=-1)
        x = x.contiguous().view(batchsize, -1)
        return x





def feature_transform_regularizer(trans):
    d = trans.size()[1]
    batchsize = trans.size()[0]
    I = torch.eye(d)[None, :, :]
    if trans.is_cuda:
        I = I.cuda()
    loss = torch.mean(torch.norm(torch.bmm(trans, trans.transpose(2,1)) - I, dim=(1,2)))
    return loss

if __name__ == '__main__':
    sim_data = Variable(torch.rand(32,3,2500))
    trans = STN3d()
    out = trans(sim_data)
    print('stn', out.size())
    print('loss', feature_transform_regularizer(out))

    sim_data_64d = Variable(torch.rand(32, 64, 2500))
    trans = STNkd(k=64)
    out = trans(sim_data_64d)
    print('stn64d', out.size())
    print('loss', feature_transform_regularizer(out))

    pointfeat = PointNetfeat(global_feat=True)
    out, _, _ = pointfeat(sim_data)
    print('global feat', out.size())

    pointfeat = PointNetfeat(global_feat=False)
    out, _, _ = pointfeat(sim_data)
    print('point feat', out.size())

    cls = PointNetCls(k = 5)
    out, _, _ = cls(sim_data)
    print('class', out.size())

    seg = PointNetDenseCls(k = 3)
    out, _, _ = seg(sim_data)
    print('seg', out.size())
scripts/model_builder/pcl/pointnet.py
RobotiXX/multimodal-fusion-network
# from typing import List

import torch
import torch.nn as nn




class PclBackbone(nn.Module):
    def __init__(
        self,        
    ):

        super().__init__()

        
        self.common = nn.Sequential(
            nn.Conv3d(in_channels=1, out_channels=8, kernel_size=3, stride=1),
            nn.LeakyReLU(),
            nn.Conv3d(in_channels=8, out_channels=16, kernel_size=5, stride=1),
            nn.LeakyReLU(),   
            nn.MaxPool3d(3, stride=(2,2,1))                   
            )

        self.ft1 = nn.Sequential(
            nn.Conv3d(in_channels=16, out_channels=24, kernel_size=5, stride=1),
            nn.LeakyReLU(),
            nn.MaxPool3d(3, stride=(2,2,1))                   
        )

        self.ft2 = nn.Sequential(
            nn.Conv3d(in_channels=24, out_channels=36, kernel_size=5, stride=1),
            nn.LeakyReLU(),   
            nn.MaxPool3d(3, stride=(2,2,1))                            
        )

        self.ft3 = nn.Sequential(
            nn.Conv3d(in_channels=36, out_channels=48, kernel_size=1, stride=1),
            nn.LeakyReLU(),    
        )                
        

    def forward(self, input):
             
        batchsize = input.size()[0]
        # print(input.size())
        feat_shared = self.common(input)

        feat_l1 = self.ft1(feat_shared)
        feat_l2 = self.ft2(feat_l1)
        feat_l3 = self.ft3(feat_l2)

        # feat_l1 = feat_l1.contiguous().view(batchsize, -1)
        # feat_l2 = feat_l2.contiguous().view(batchsize, -1)
        feat_l3 = feat_l3.contiguous().view(batchsize, -1)        
        
        return feat_l3

scripts/model_builder/pcl/pointnet_backbone.py
RobotiXX/multimodal-fusion-network

from __future__ import print_function
import torch
import torch.nn as nn
import torch.nn.parallel
import torch.utils.data
# from torch.autograd import Variabl
# import numpy as np
import torch.nn.functional as F
from .pointnet import PointNetfeat


class PointNetDenseFusionModel(nn.Module):
    def __init__(self, k = 64, feature_transform=False):
        super(PointNetDenseFusionModel, self).__init__()
        self.k = k
        self.feature_transform=feature_transform
        self.feat = PointNetfeat(global_feat=False, feature_transform=feature_transform)
        self.conv1 = torch.nn.Conv1d(1088, 12, 1)
        self.conv2 = torch.nn.Conv1d(12, 10, 1)
        self.conv3 = torch.nn.Conv1d(10, 8, 1)
        self.conv4 = torch.nn.Conv1d(8, 6, 1)

    def forward(self, x):
        batchsize = x.size()[0]
        n_pts = x.size()[2]
        x, trans, trans_feat = self.feat(x)
        x = F.relu(self.conv1(x))
  
        cv2 = F.relu(self.conv2(x))

        cv3 = F.relu(self.conv3(cv2))

        x = self.conv4(cv3)
        # x = x.transpose(2,1).contiguous()
        # x = F.log_softmax(x.view(-1,self.k), dim=-1)
        x = x.contiguous().view(batchsize, -1)
        cv2 = cv2.contiguous().view(batchsize, -1)
        cv3 = cv3.contiguous().view(batchsize, -1)

        return x, (cv2, cv3)

scripts/model_builder/pcl/pointnet_fusion.py
RobotiXX/multimodal-fusion-network
# -*- coding:utf-8 -*-
# author: Xinge
# @file: segmentator_3d_asymm_spconv.py

import numpy as np
import spconv.pytorch as spconv
from torch import nn


def conv3x3(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=3, stride=stride,
                             padding=1, bias=False, indice_key=indice_key)


def conv1x3(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=(1, 3, 3), stride=stride,
                             padding=(0, 1, 1), bias=False, indice_key=indice_key)


def conv1x1x3(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=(1, 1, 3), stride=stride,
                             padding=(0, 0, 1), bias=False, indice_key=indice_key)


def conv1x3x1(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=(1, 3, 1), stride=stride,
                             padding=(0, 1, 0), bias=False, indice_key=indice_key)


def conv3x1x1(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=(3, 1, 1), stride=stride,
                             padding=(1, 0, 0), bias=False, indice_key=indice_key)


def conv3x1(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=(3, 1, 3), stride=stride,
                             padding=(1, 0, 1), bias=False, indice_key=indice_key)


def conv1x1(in_planes, out_planes, stride=1, indice_key=None):
    return spconv.SubMConv3d(in_planes, out_planes, kernel_size=1, stride=stride,
                             padding=1, bias=False, indice_key=indice_key)


class ResContextBlock(nn.Module):
    def __init__(self, in_filters, out_filters, kernel_size=(3, 3, 3), stride=1, indice_key=None):
        super(ResContextBlock, self).__init__()
        self.conv1 = conv1x3(in_filters, out_filters, indice_key=indice_key + "bef")
        self.bn0 = nn.BatchNorm1d(out_filters)
        self.act1 = nn.LeakyReLU()

        self.conv1_2 = conv3x1(out_filters, out_filters, indice_key=indice_key + "bef")
        self.bn0_2 = nn.BatchNorm1d(out_filters)
        self.act1_2 = nn.LeakyReLU()

        self.conv2 = conv3x1(in_filters, out_filters, indice_key=indice_key + "bef")
        self.act2 = nn.LeakyReLU()
        self.bn1 = nn.BatchNorm1d(out_filters)

        self.conv3 = conv1x3(out_filters, out_filters, indice_key=indice_key + "bef")
        self.act3 = nn.LeakyReLU()
        self.bn2 = nn.BatchNorm1d(out_filters)

        self.weight_initialization()

    def weight_initialization(self):
        for m in self.modules():
            if isinstance(m, nn.BatchNorm1d):
                nn.init.constant_(m.weight, 1)
                nn.init.constant_(m.bias, 0)

    def forward(self, x):
        shortcut = self.conv1(x)
        shortcut.features = self.act1(shortcut.features)
        shortcut.features = self.bn0(shortcut.features)

        shortcut = self.conv1_2(shortcut)
        shortcut.features = self.act1_2(shortcut.features)
        shortcut.features = self.bn0_2(shortcut.features)

        resA = self.conv2(x)
        resA.features = self.act2(resA.features)
        resA.features = self.bn1(resA.features)

        resA = self.conv3(resA)
        resA.features = self.act3(resA.features)
        resA.features = self.bn2(resA.features)
        resA.features = resA.features + shortcut.features

        return resA


class ResBlock(nn.Module):
    def __init__(self, in_filters, out_filters, dropout_rate, kernel_size=(3, 3, 3), stride=1,
                 pooling=True, drop_out=True, height_pooling=False, indice_key=None):
        super(ResBlock, self).__init__()
        self.pooling = pooling
        self.drop_out = drop_out

        self.conv1 = conv3x1(in_filters, out_filters, indice_key=indice_key + "bef")
        self.act1 = nn.LeakyReLU()
        self.bn0 = nn.BatchNorm1d(out_filters)

        self.conv1_2 = conv1x3(out_filters, out_filters, indice_key=indice_key + "bef")
        self.act1_2 = nn.LeakyReLU()
        self.bn0_2 = nn.BatchNorm1d(out_filters)

        self.conv2 = conv1x3(in_filters, out_filters, indice_key=indice_key + "bef")
        self.act2 = nn.LeakyReLU()
        self.bn1 = nn.BatchNorm1d(out_filters)

        self.conv3 = conv3x1(out_filters, out_filters, indice_key=indice_key + "bef")
        self.act3 = nn.LeakyReLU()
        self.bn2 = nn.BatchNorm1d(out_filters)

        if pooling:
            if height_pooling:
                self.pool = spconv.SparseConv3d(out_filters, out_filters, kernel_size=3, stride=2,
                                                padding=1, indice_key=indice_key, bias=False)
            else:
                self.pool = spconv.SparseConv3d(out_filters, out_filters, kernel_size=3, stride=(2, 2, 1),
                                                padding=1, indice_key=indice_key, bias=False)
        self.weight_initialization()

    def weight_initialization(self):
        for m in self.modules():
            if isinstance(m, nn.BatchNorm1d):
                nn.init.constant_(m.weight, 1)
                nn.init.constant_(m.bias, 0)

    def forward(self, x):
        shortcut = self.conv1(x)
        shortcut.features = self.act1(shortcut.features)
        shortcut.features = self.bn0(shortcut.features)

        shortcut = self.conv1_2(shortcut)
        shortcut.features = self.act1_2(shortcut.features)
        shortcut.features = self.bn0_2(shortcut.features)

        resA = self.conv2(x)
        resA.features = self.act2(resA.features)
        resA.features = self.bn1(resA.features)

        resA = self.conv3(resA)
        resA.features = self.act3(resA.features)
        resA.features = self.bn2(resA.features)

        resA.features = resA.features + shortcut.features

        if self.pooling:
            resB = self.pool(resA)
            return resB, resA
        else:
            return resA



class Asymm_3d_spconv(nn.Module):
    def __init__(self,
                 output_shape = [480, 360, 32],
                 use_norm=True,
                 num_input_features=16,
                 nclasses=20, n_height=32, strict=False, init_size=32):
        super(Asymm_3d_spconv, self).__init__()
        self.nclasses = nclasses
        self.nheight = n_height
        self.strict = False

        sparse_shape = np.array(output_shape)
        # sparse_shape[0] = 11
        print(sparse_shape)
        self.sparse_shape = sparse_shape

        self.downCntx = ResContextBlock(num_input_features, init_size, indice_key="pre")
        self.resBlock2 = ResBlock(init_size, 2 * init_size, 0.2, height_pooling=True, indice_key="down2")
        self.resBlock3 = ResBlock(2 * init_size, 4 * init_size, 0.2, height_pooling=True, indice_key="down3")
        self.resBlock4 = ResBlock(4 * init_size, 8 * init_size, 0.2, pooling=True, height_pooling=False,
                                  indice_key="down4")
        self.resBlock5 = ResBlock(8 * init_size, 16 * init_size, 0.2, pooling=True, height_pooling=False,
                                  indice_key="down5")
        



    def forward(self, voxel_features, coors, batch_size):
        # x = x.contiguous()
        coors = coors.int()
        # import pdb
        # pdb.set_trace()
        ret = spconv.SparseConvTensor(voxel_features, coors, self.sparse_shape,
                                      batch_size)
        ret = self.downCntx(ret)
        down1c, _ = self.resBlock2(ret)
        down2c, _ = self.resBlock3(down1c)
        down3c, _ = self.resBlock4(down2c)
        down4c, _ = self.resBlock5(down3c)

        return down4c

scripts/model_builder/pcl/spconv.py
RobotiXX/multimodal-fusion-network
import torch
import torch.nn as nn

class CustomTransformerModelPcl(nn.Module):
    def __init__(self, hidden_dim = 1024+128, num_layers = 2, nhead=4):
        super(CustomTransformerModelPcl, self).__init__()                
        self.transformer_encoder_layer = nn.TransformerEncoderLayer(d_model=hidden_dim, nhead=nhead, batch_first=True)
        self.transformer_encoder = nn.TransformerEncoder(self.transformer_encoder_layer, num_layers=num_layers)
    

    def forward(self, x):
        # linear_output = self.linear(x)
        transformer_output = self.transformer_encoder(x)
        # output = self.fc(transformer_output)
        return transformer_output

scripts/model_builder/pcl/tf_pcl.py
RobotiXX/multimodal-fusion-network
#!/usr/bin/env python

from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist, Pose
from nav_msgs.msg import Path, Odometry
import rospy
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal

class Robot_config():
    def __init__(self):
        self.X = 0 # inertia frame
        self.Y = 0
        self.PSI = 0
        self.global_path = []
        self.gx = 0 # body frame
        self.gy = 0
        self.gp = 0
        self.los = 3
        # self.los = 5
    
    def get_robot_status(self, msg):
        q1 = msg.pose.pose.orientation.x
        q2 = msg.pose.pose.orientation.y
        q3 = msg.pose.pose.orientation.z
        q0 = msg.pose.pose.orientation.w
        self.X = msg.pose.pose.position.x
        self.Y = msg.pose.pose.position.y
        self.PSI = np.arctan2(2 * (q0*q3 + q1*q2), (1 - 2*(q2**2+q3**2)))
        #print(self.X, self.Y, self.PSI)
    
    def get_global_path(self, msg):
        #print(msg.poses)
        # self.global_path = []
        gp = []
        for pose in msg.poses:
            gp.append([pose.pose.position.x, pose.pose.position.y])
            # self.global_path.append([pose.pose.position.x, pose.pose.position.y])
        #print(len(self.global_path))
        gp = np.array(gp)
        x = gp[:,0]
        # xhat = x
        try:
            xhat = scipy.signal.savgol_filter(x, 19, 3)
        except:
            xhat = x
        y = gp[:,1]
        # yhat = y
        try: 
            yhat = scipy.signal.savgol_filter(y, 19, 3)
        except: 
            yhat = y
        # plt.figure()
        # plt.plot(xhat, yhat, 'k', linewidth=1)
        # plt.axis('equal')
        # plt.savefig("/home/xuesu/gp_plt.png")
        # plt.close()
        gphat = np.column_stack((xhat, yhat))
        gphat.tolist()
        self.global_path = gphat
        # print(self.global_path)



def transform_lg(wp, X, Y, PSI):
    R_r2i = np.matrix([[np.cos(PSI), -np.sin(PSI), X], [np.sin(PSI), np.cos(PSI), Y], [0, 0, 1]])
    R_i2r = np.linalg.inv(R_r2i)
    #print(R_r2i)
    pi = np.matrix([[wp[0]], [wp[1]], [1]])
    pr = np.matmul(R_i2r, pi)
    #print(pr) 
    lg = np.array([pr[0,0], pr[1, 0]])
    #print(lg)
    return lg
    
        
if __name__ == '__main__':
    robot_config = Robot_config()
    rospy.init_node('jackal_global_plan', anonymous=True)
    sub_robot = rospy.Subscriber("/odometry/filtered", Odometry, robot_config.get_robot_status)
    sub_gp = rospy.Subscriber("/move_base/TrajectoryPlannerROS/global_plan", Path, robot_config.get_global_path)

    lg = Pose()
    pub_lg = rospy.Publisher('local_goal', Pose, queue_size=10)
    while not rospy.is_shutdown():
        gp = robot_config.global_path
        X = robot_config.X
        Y = robot_config.Y
        PSI = robot_config.PSI
        los = robot_config.los
        
        #if len(gp)==0:
        lg_x = 0
        lg_y = 0
        #else:
        if len(gp)>0:
            lg_flag = 0
            for wp in gp:
                dist = (np.array(wp)-np.array([X, Y]))**2
                dist = np.sum(dist, axis=0)
                dist = np.sqrt(dist)
                if dist > los:
                    lg_flag = 1
                    lg = transform_lg(wp, X, Y, PSI)
                    lg_x = lg[0]
                    lg_y = lg[1]
                    break
            if lg_flag == 0:
                lg = transform_lg(gp[-1], X, Y, PSI)
                lg_x = lg[0]
                lg_y = lg[1]
                
        # print(lg_x, lg_y)
        local_goal = Pose()
        local_goal.position.x = lg_x
        local_goal.position.y = lg_y
        local_goal.orientation.w = 1
        pub_lg.publish(local_goal)
        

scripts/movebase.py
RobotiXX/multimodal-fusion-network
from comet_ml import Experiment
import torch
import os
import numpy as np
import math
from random import shuffle

from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from model_builder.multimodal.fusion_net import BcFusionModel
from torch.utils.data import DataLoader
from tqdm.auto import tqdm
import matplotlib.pyplot as plt
import coloredlogs, logging
from model_builder.multimodal.multi_net import MultiModalNet
from data_builder.cmd_scaler import transform_to_gt_scale
from data_builder.gaussian_weights import get_gaussian_weights


from torch.optim.lr_scheduler import MultiStepLR,CosineAnnealingWarmRestarts

# Create an experiment with your api key
experiment = Experiment(
    #api_key="",    
    project_name="multimodal-net-with-rnn",
    workspace="bhabaranjan",
)

experiment.add_tag('Multimodal-end-t0-end')
experiment.log_asset('/scratch/bpanigr/fusion-network/scripts/model_builder/multimodal/multi_net.py')

coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

min_val_error = 100000

val_dict = {}

# root_path = '/home/ranjan/Workspace/my_works/fusion-network/recorded-data'
# model_storage_path = '/home/ranjan/Workspace/my_works/fusion-network/scripts'

root_path = '/scratch/bpanigr/fusion-network/recorded-data'
model_storage_path = '/scratch/bpanigr/model_weights/end-to-end'

weights = get_gaussian_weights(2,1.3)
weights = weights[:,:4] 
weights = np.concatenate([weights, weights], axis=1)
weights = torch.tensor(weights)
weights = weights.to(device)

def get_loss_fun(loss_type = None):
    if loss_type == 'mse':
        return torch.nn.MSELoss()
    else:
       return torch.nn.L1Loss()

def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=False, prefetch_factor=2, num_workers=20)
    return data_loader
   

def get_loss(loss_fn, pts, gt_pts, data_src):
    error =  loss_fn(pts, gt_pts)     
    
    if data_src == 'validation':     
        experiment.log_metric(name = str('way_pts'+data_src), value= error.item())    
    else:
        experiment.log_metric(name = str('way_pts'+data_src), value=error.item())      
    return error

def experiment_logger(file_path, prefix, suffix, value, epoch):
    # global experiment
    file_name = file_path.split('/')[-1]
    experiment.log_metric(name = prefix+file_name+suffix, value=value, epoch= epoch+1)
    return 


def run_validation(val_files, model, batch_size, epoch, optim):
       print("Running Validation..\n")
       running_error = []
       loss = get_loss_fun()
       model.eval()
       with torch.no_grad():
        for val_file in val_files:
            val_loader = None
            if val_file not in val_dict:
                val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
                val_dict[val_file] = val_loader
            else:
                val_loader = val_dict[val_file]

            loss_path_fsn =  []
            loss_cmd_fsn =  []
            loss_path_img =  []
            loss_cmd_img =  []            
            loss_path_pcl = []
            loss_cmd_pcl = []
            per_file_total_loss = []

            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd_vel) in tqdm(enumerate(val_loader)):
                stacked_images = stacked_images.to(device)
                pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                gt_pts = gt_pts.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                
                fsn_global_path_feats, fusion_vel, img_path, img_vel, pcl_path, pcl_vel = model(stacked_images, pcl, local_goal)

                # print(gt_cmd_vel.shape)
                # print(pred_cmd.shape)

                gt_cmd_vel = transform_to_gt_scale(gt_cmd_vel, device)                
                fusion_vel = transform_to_gt_scale(fusion_vel, device)  
                img_vel = transform_to_gt_scale(img_vel, device)  
                pcl_vel = transform_to_gt_scale(pcl_vel, device)  
                
                fusion_vel = get_loss(loss, fusion_vel, gt_cmd_vel, 'validation')
                error_img_cmd = get_loss(loss, img_vel, gt_cmd_vel, 'validation')
                error_pcl_cmd = get_loss(loss, pcl_vel, gt_cmd_vel, 'validation')
                
                error_fusion_path = get_loss(loss, fsn_global_path_feats/ weights, gt_pts/ weights, 'validation')
                error_img_path = get_loss(loss, img_path/ weights, gt_pts/ weights, 'validation')
                error_pcl_path = get_loss(loss, pcl_path/ weights, gt_pts/ weights, 'validation')
                

                error_total = error_fusion_path + fusion_vel + error_img_path + error_img_cmd + error_pcl_path + error_pcl_cmd


                loss_path_fsn.append(error_fusion_path.item())
                loss_cmd_fsn.append(fusion_vel.item())

                loss_path_img.append(error_img_path.item())
                loss_cmd_img.append(error_img_cmd.item())
                
                loss_path_pcl.append(error_pcl_path.item())
                loss_cmd_pcl.append(error_pcl_cmd.item())

                per_file_total_loss.append(error_total.item())
                                                        

            experiment_logger(val_file, "val_", "_fusion", np.average(loss_path_fsn), epoch)
            experiment_logger(val_file, "val_", "_fusion_cmd", np.average(loss_cmd_fsn), epoch)

            experiment_logger(val_file, "val_", "_img", np.average(loss_path_img), epoch)
            experiment_logger(val_file, "val_", "_img_cmd", np.average(loss_cmd_img), epoch)

            experiment_logger(val_file, "val_", "_pcl", np.average(loss_path_pcl), epoch)
            experiment_logger(val_file, "val_", "_pcl_cmd", np.average(loss_cmd_pcl), epoch)                               

            print(f'============= Perfile loss fns: {np.average(loss_path_fsn), np.average(loss_cmd_fsn)}  img: {np.average(loss_path_img), np.average(loss_cmd_img)}  pcl: {np.average(loss_path_pcl), np.average(loss_cmd_pcl)}\n')
            running_error.append(np.average(per_file_total_loss))
        
        avg_loss_on_validation = np.average(running_error)
        # print(f'epoch:------>{epoch}')
        if (epoch+1) % 10 == 0 and (epoch+1) > 30:
            print(f"saving model weights at validation error {avg_loss_on_validation}")
            torch.save({
            'model_state_dict': model.state_dict(),
            'optimizer_state_dict': optim.state_dict(),
            }, f'{model_storage_path}/tf8_img_changed{epoch+1}_{avg_loss_on_validation}.pth')

        print(f'=========================> Average Validation error is:   { avg_loss_on_validation } \n')
        return avg_loss_on_validation            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = get_loss_fun()
    model = MultiModalNet()    

    model.to(device)
    optim = torch.optim.Adam(model.parameters(), lr=0.00000888)     
    # run_validation(val_dirs, model, batch_size, 0, optim)
    # return
    
    # ckpt = torch.load('/scratch/bpanigr/model_weights/end-to-end/v2_end_to_end_velocities_100_0.642947574742124.pth')
    # model.load_state_dict(ckpt['model_state_dict'])
    # optim.load_state_dict(ckpt['optimizer_state_dict'])

    scheduler = MultiStepLR(optim, milestones= [30,80,130,180,230,300], gamma=.75)

    data_dict = {}
    for epoch in range(num_epochs):
        num_files = 0
        lr = scheduler.get_last_lr()        
        experiment.log_metric( name = "Learning Rate Decay", value = lr, epoch= epoch+1)
        running_loss = []
        shuffle(train_files)  
        # clear_dict(data_dict,epoch)      
        model.train()
        for train_file in train_files:        
            train_loader = None 
            print(train_file)
            if train_file not in data_dict:
                train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )   
                data_dict[train_file] = train_loader
            else:
                train_loader = data_dict[train_file]

            num_files += 1

            loss_path_fsn =  []
            loss_cmd_fsn =  []
            loss_path_img =  []
            loss_cmd_img =  []            
            loss_path_pcl = []
            loss_cmd_pcl = []
            per_file_total_loss = []

            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd_vel) in enumerate(train_loader):
                
                stacked_images = stacked_images.to(device)
                pcl = pcl.to(device)
                local_goal= local_goal.to(device)                
                gt_cmd_vel= gt_cmd_vel.to(device)
                gt_pts = gt_pts.to(device)

                optim.zero_grad()
                
                fsn_global_path_feats, fusion_vel, img_path, img_vel, pcl_path, pcl_vel = model(stacked_images, pcl, local_goal)
                
                
                error_fusion_path = get_loss(loss, fsn_global_path_feats, gt_pts,'train_pcl')
                error_fusion_cmd = get_loss(loss, fusion_vel, gt_cmd_vel,'train_pcl')
                
                error_img_path = get_loss(loss, img_path, gt_pts, 'train_pcl')
                erro_img_cmd = get_loss(loss, img_vel, gt_cmd_vel, 'train_pcl')
                
                error_pcl_path = get_loss(loss, pcl_path, gt_pts, 'train_pcl')
                error_pcl_cmd = get_loss(loss, pcl_vel, gt_cmd_vel, 'train_pcl')

                
                error_total = error_fusion_path + error_fusion_cmd + error_img_path + erro_img_cmd + error_pcl_path + error_pcl_cmd

                
                loss_path_fsn.append(error_fusion_path.item())
                loss_cmd_fsn.append(error_fusion_cmd.item())

                loss_path_img.append(error_img_path.item())
                loss_cmd_img.append(erro_img_cmd.item())
                
                loss_path_pcl.append(error_pcl_path.item())
                loss_cmd_pcl.append(error_pcl_cmd.item())

                per_file_total_loss.append(error_total.item())

                error_total.backward()
                optim.step()

                
                print(f' Step is:  {index}  Loss is fusion :: {error_fusion_path.item(), error_fusion_cmd.item()}  img:{error_img_path.item(), erro_img_cmd.item() } pcl:{error_pcl_path.item(), error_pcl_cmd.item()} \n')
            
            # experiment.log_metric(name = str(train_file.split('/')[-1]+ " mod:" +'img'), value=np.average(per_file_loss_ǐmage), epoch= epoch+1)

            experiment_logger(train_file, "", " mod:fusion", np.average(loss_path_fsn), epoch)
            experiment_logger(train_file, "", " mod:fusion_cmd", np.average(loss_cmd_fsn), epoch)

            experiment_logger(train_file, "", " mod:img", np.average(loss_path_img), epoch)
            experiment_logger(train_file, "", " mod:img_cmd", np.average(loss_cmd_img), epoch)

            experiment_logger(train_file, "", " mod:pcl", np.average(loss_path_pcl), epoch)
            experiment_logger(train_file, "", " mod:pcl_cmd", np.average(loss_cmd_pcl), epoch)

            # experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'pcl'), value=np.average(per_file_loss_pcl), epoch= epoch+1)
            # experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'fusion'), value=np.average(per_file_loss_fusion), epoch= epoch+1)
            print(f' \n ========== perfile loss fns: {np.average(loss_path_fsn), np.average(loss_cmd_fsn) }  img: {np.average(loss_path_img), np.average(loss_cmd_img)}  pcl: {np.average(loss_path_pcl), np.average(loss_cmd_pcl)}    ==================\n')      
            running_loss.append(np.average(per_file_total_loss))   
            
        scheduler.step()
        
        print(f'================== epoch is: {epoch} and error is: {np.average(running_loss)}\n')

        if (epoch+1) % 2 == 0:
            val_error = run_validation(val_dirs, model, batch_size, epoch, optim)
            experiment.log_metric( name = "Avg Validation loss", value = val_error, epoch= epoch+1)
        # val_error_at_epoch.append(val_error)
        experiment.log_metric( name = "Avg Training loss", value = np.average(running_loss), epoch= epoch+1)
        


def main():
    train_path = f'{root_path}/train'
    validation_path = f'{root_path}/val'
    
    train_dirs = [ os.path.join(train_path, dir) for dir in os.listdir(train_path)]
    val_dirs = [ os.path.join(validation_path, dir) for dir in os.listdir(validation_path)]

    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136021_wt')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/138181_wt')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135968_wt_at')
    # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136514_sw_wt_sc')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135967_at')

    batch_size = 80
    epochs = 450
    run_training(train_dirs, val_dirs, batch_size, epochs)

main()





scripts/multimodal.py
RobotiXX/multimodal-fusion-network
import pickle
import os

from torch.utils.data import Dataset

class IndexDataset(Dataset):

    def __init__(self, data):        

        self.content = data

    def __len__(self):
        # As images and point clouds will be in sets of 4
        return int( (len(self.content.keys())-1) / 4)
    
    def __getitem__(self, offset_index) :
        # We are taking 4 sequential images, point clouds each time to account for temporal variation
        start_index = offset_index * 4
        end_index = start_index + 3

        # Get data from respective index
        prev_cmd_vel = self.content[end_index]['prev_cmd_vel']
        local_goal = self.content[end_index]['local_goal']
        
        # Image paths
        image_paths = self.content['images']
        
        # only keep points that are under 5 + 1 (delta) meters from the robot
        point_clouds = []
        # print(list(self.content.keys()), start_index, end_index)
        for point_snapshot in range(start_index, end_index+1):
            filtered_points = []            
            for point in self.content[point_snapshot]['point_cloud']:
                if point[0] >= -1 and point[0] <= 5 and point[1]>=-3 and point[1]<=3:
                    filtered_points.append(point)
            
            point_clouds.append(filtered_points)                


        return (image_paths, point_clouds, local_goal, prev_cmd_vel)
scripts/on_robot/indexer.py
RobotiXX/multimodal-fusion-network
#!/usr/bin/env python
# coding: utf-8
import numpy as np
import rospy
import subprocess
import message_filters
import sensor_msgs.point_cloud2 as pc2
import cv2
import sys
import os
import pickle

from sensor_msgs.msg import PointCloud2, CompressedImage, Joy
from nav_msgs.msg import Odometry
from pathlib import Path
from collections import OrderedDict

counter = {'index': 0, 'sub-sampler': 1}
previous_rbt_location = OrderedDict()
local_goal = {}
previous_velocities = []
play_back_snapshot = {}

rosbag_path = sys.argv[1]

bag_file_name = rosbag_path.split('/')[-1]

record_storage_path = os.path.join('../../recorded-data', bag_file_name)        
os.makedirs(record_storage_path, exist_ok=True)

def getJoystickValue(x, scale, kDeadZone=0.02):
    if kDeadZone != 0.0 and abs(x) < kDeadZone:
        return 0.0
    return ((x - np.sign(x) * kDeadZone) / (1.0 - kDeadZone) * scale)

def odom_callback(odom):
    position = odom.pose.pose.position
    
    keys = list(previous_rbt_location.keys())
    for key in keys:
        # if the current odom position is with in 10 meter radius of a previous position
        # then use current point as a local goal of the previous position(robot's prev location)
        # TODO: should be done as a perpendicular intersection instead of delta approximation of 2

        if ((previous_rbt_location[key][0] - position.x) ** 2 + (previous_rbt_location[key][1] - position.y) ** 2 ) >= 0.2299:
            if  len(play_back_snapshot[key]["local_goal"]) < 12:
                play_back_snapshot[key]["local_goal"].append((position.x, position.y))
                previous_rbt_location[key][0] = position.x
                previous_rbt_location[key][1] = position.y
                # break
            else:    # print("local goal foudn for index", robot_pos[2])                
                previous_rbt_location.pop(key)
        else:
            break

def get_lidar_points(lidar):
    point_cloud = []
    for point in pc2.read_points(lidar, skip_nans=True, field_names=('x', 'y', 'z')):
        pt_x = point[0]
        pt_y = point[1]
        pt_z = point[2]
        point_cloud.append([pt_x, pt_y, pt_z])
    return point_cloud


def get_prev_cmd_val():
    return previous_velocities


def store_image(rgb_image, idx):
    np_arr = np.frombuffer(rgb_image.data, np.uint8)
    image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
    # image = cv2.resize(image, (264, 300))
    image_path = os.path.join(record_storage_path, str(idx)+".jpg")
    cv2.imwrite(image_path, image)

def joy_call_back(joy):
    joy_axes = joy.axes
    previous_velocities.insert(0, (
            getJoystickValue(joy_axes[4], -1.6),
            getJoystickValue(joy_axes[3], -1.6),
            getJoystickValue(joy_axes[0], -np.deg2rad(90.0), kDeadZone=0.0),
        ))
    while len(previous_velocities) > 22:
        previous_velocities.pop()

def aprrox_sync_callback(lidar, rgb, odom, joy):
    pos = odom.pose.pose.position
    cmd_vel = odom.twist.twist
    joy_axes = joy.axes
    # This function is called at 10Hz
    # Subsampling at each 5th second approx
    # print(f'counter: {counter["sub-sampler"]}')
    if counter['sub-sampler'] % 2 == 0:
        # TODO: these 4 values will be pickled at index counter['index'] except image
        store_image(rgb, counter['index'])
        point_cloud = get_lidar_points(lidar)
        prev_cmd_vel = get_prev_cmd_val().copy()
        prev_cmd_vel.pop(0)
        prev_cmd_vel.pop(0)
        orientation = odom.pose.pose.orientation
        robot_pos = ([pos.x, pos.y, pos.z], [orientation.x, orientation.y, orientation.z, orientation.w], counter['index'])
        # Record data at current point
        gt_cmd_vel = (
            getJoystickValue(joy_axes[4], -1.6),
            getJoystickValue(joy_axes[3], -1.6),
            getJoystickValue(joy_axes[0], -np.deg2rad(90.0), kDeadZone=0.0),
        )
        # print(f'ground truth velocity: {gt_cmd_vel}\n\n')
        # print("here")
        play_back_snapshot[counter['index']] = {
            "point_cloud": point_cloud,
            # "prev_cmd_vel": prev_cmd_vel,
            "robot_position": robot_pos,
            "gt_cmd_vel": gt_cmd_vel,
            "local_goal": []
        }
        previous_rbt_location[counter['index']] =[pos.x, pos.y]

        counter['index'] += 1
        counter['sub-sampler'] = 0

    counter['sub-sampler'] += 1    


rospy.init_node('listen_record_data', anonymous=True)


rosbag_play_process = subprocess.Popen(
    ['rosbag', 'play', '--clock', rosbag_path])

lidar = message_filters.Subscriber('/velodyne_points', PointCloud2)
rgb = message_filters.Subscriber('/image_raw/compressed', CompressedImage)
odom = message_filters.Subscriber('/odom', Odometry)
joy = message_filters.Subscriber('/joystick', Joy)



odom.registerCallback(odom_callback)
joy.registerCallback(joy_call_back)

ts = message_filters.ApproximateTimeSynchronizer([lidar, rgb, odom, joy], 100, 0.05, allow_headerless=True)
ts.registerCallback(aprrox_sync_callback)

while not rospy.is_shutdown():
    if rosbag_play_process.poll() is not None:
        print('Rosbag play has stopped, saving the data at:', record_storage_path)                
        pickle_file = os.path.join(record_storage_path, "snapshot.pickle")
        
        with open(pickle_file, 'wb') as handle:
            pickle.dump(play_back_snapshot, handle, protocol=pickle.HIGHEST_PROTOCOL)
        exit(0)

scripts/parser/bagfile_parser.py
RobotiXX/multimodal-fusion-network
import os
import subprocess

file_list = os.listdir('../../bagfiles/')

print("Number of files:", len(file_list))

for idx, file in enumerate(file_list):    
    print("\n\n\nProcessing Index:", idx)    
    
    if file != '.DS_Store':
        process = subprocess.Popen([
            'python3', 'bagfile_parser.py', os.path.join(str('../../bagfiles'),file)
        ])
        print(">>>>>>>>>>>>>>>>>> Processing file:", file)
    
        while True:
            if process.poll() is not None:
                print("\n\n\n >>>>>>>>>>>>>>>>>> Processed finshed:", file)
                break        
    else:
        print('Skipping file:', file)
scripts/parser/parse_runner.py
RobotiXX/multimodal-fusion-network
import rosbag
import os

def check_topics_in_rosbag(rosbag_file, topics):
    # Open the ROS bag file
    bag = rosbag.Bag(rosbag_file)

    # Get the list of available topics in the bag file
    bag_topics = bag.get_type_and_topic_info().topics.keys()

    # Check if all the specified topics are present in the bag file
    missing_topics = []
    for topic in topics:
        if topic not in bag_topics:
            missing_topics.append(topic)

    # Close the ROS bag file
    bag.close()

    return missing_topics

# Example usage
bagfile_location = "../../bagfiles"
topics = ['/odom', '/image_raw/compressed', '/velodyne_points']


for path in os.listdir(bagfile_location):
    missing_topics = check_topics_in_rosbag(os.path.join(bagfile_location,path), topics)

    if len(missing_topics) == 0:
        print(f"All topics are present in the ROS bag file: {path}")
    else:
        print(f"Missing topics in the ROS bag file: {path}")
        for topic in missing_topics:
            print(topic)

scripts/parser/topic_validator.py
RobotiXX/multimodal-fusion-network
import torch
from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from model_builder.pcl.net import BCModelPcl
from torch.utils.data import DataLoader
# import matplotlib.pyplot as plt

device = "cuda" if torch.cuda.is_available() else "cpu"



root = '/Users/bhabaranjanpanigrahi/Research/Code/fusion-network/recorded-data/136021.bag'

indexer = IndexDataset(root)
train_dataset = ApplyTransformation(indexer)
batch_size=16
train_loader = DataLoader(train_dataset, batch_size=batch_size, drop_last=True)

running_loss = []
loss = torch.nn.MSELoss()
model = BCModelPcl().to(device)
optim = torch.optim.Adam(model.parameters(), lr=0.001)

for epoch in range(10):
    for index, (stacked_images, grid_indexes, points_clouds, local_goal, prev_cmd_vel, gt_cmd_vel) in enumerate(train_loader):
        
        grid_indexes = grid_indexes.to(device)
        points_clouds = points_clouds.to(device)
        
        local_goal= local_goal.to(device)
        prev_cmd_vel= prev_cmd_vel.to(device)
        gt_cmd_vel= gt_cmd_vel.to(device)
        # print(f"{gt_cmd_vel.shape = }")

        pred_cmd_vel = model(points_clouds, grid_indexes, local_goal, prev_cmd_vel, batch_size)
        # print(f"{pred_cmd_vel.shape = }")
        error  = loss(pred_cmd_vel, gt_cmd_vel)
        optim.zero_grad()
        error.backward()
        optim.step()

        print(f'step is: {index} and error is: {error.item()}')

        running_loss.append(error.item())

    print(f'epoch is: {epoch} and error is: {sum(running_loss)/len(running_loss)}')

# plt.plot(range(10),running_loss[:10])
scripts/pcl_main.py
RobotiXX/multimodal-fusion-network
from comet_ml import Experiment
import torch
import os
import numpy as np
from random import shuffle

from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from data_builder.gaussian_weights import get_gaussian_weights
from data_builder.cmd_scaler import transform_to_gt_scale
from model_builder.multimodal.fusion_net import BcFusionModel
from torch.utils.data import DataLoader
from tqdm.auto import tqdm
import matplotlib.pyplot as plt
import coloredlogs, logging
from model_builder.image.image_head import ImageHeadMLP
import math

from torch.optim.lr_scheduler import MultiStepLR, CosineAnnealingWarmRestarts, CyclicLR

# Create an experiment with your api key
experiment = Experiment(
    #api_key="",
    # project_name= "test",
    project_name="image_only",
    workspace="bhabaranjan",
)

experiment.add_tag('rnn-image-model')

coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

min_val_error = 100000

val_dict = {}

model_storage_path = '/scratch/bpanigr/model_weights/unimodals'

weights = get_gaussian_weights(2,1.3)
weights = weights[:,:4] 
weights = np.concatenate([weights, weights], axis=1)
weights = torch.tensor(weights)
weights = weights.to(device)


def clear_dict(dict, epoch):
    
    if epoch % 25 == 0:
        dict.clear()

    return

def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=False, prefetch_factor=2,num_workers=8)
    return data_loader

def get_loss_prev(loss_fn, lin_vel, angular_vel, gt_lin, gt_angular, data_src):
    lin_error =  loss_fn(lin_vel, gt_lin) 
    anglr_error = loss_fn(angular_vel, gt_angular)
    error = lin_error + anglr_error
    
    lin_err_val = lin_error.item()
    anglr_error_val = anglr_error.item()

    experiment.log_metric(name = str('line_error_'+data_src), value= lin_err_val)
    experiment.log_metric(name = str('anglr_error_'+data_src), value=anglr_error_val)


    return error

def get_loss(loss_fn, pts, gt_pts, data_src):
    error =  loss_fn(pts, gt_pts)

    if data_src == 'validation':     
        experiment.log_metric(name = str('way_pts'+data_src), value= error.item())    
    else:
        experiment.log_metric(name = str('way_pts'+data_src), value=error.item())    
    return error

def std_print(file,label, value):
    print(f'{file} {label}: {value}')

def run_validation(val_files, model, batch_size, epoch, optim):
       print("Running Validation..\n")
       running_error = []
       loss = torch.nn.L1Loss()
       model.eval()
       with torch.no_grad():
        for val_file in val_files:
            
            val_loader = None
            if val_file not in val_dict:
                val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
                val_dict[val_file] = val_loader
            else:
                val_loader = val_dict[val_file]

            per_file_loss_fusion  = []
            per_file_loss_ǐmage = []
            per_file_loss_pcl = []
            per_file_loss_pcl_cmd = []
            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd) in tqdm(enumerate(val_loader)):
                stacked_images = stacked_images.to(device)
                # pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                
                gt_pts= gt_pts.to(device)
                gt_cmd= gt_cmd.to(device)
                
                pts, vel = model(stacked_images, local_goal)

                                
                vel = transform_to_gt_scale(vel, device)                
                gt_cmd = transform_to_gt_scale(gt_cmd, device)  

                # print('\nstart----')
                # print(pcl_lin)
                # print(pcl_anglr)
 
                # print(gt_x)
                # print(gt_y)
                # print('end\n')
                
                # error_fusion = get_loss(loss, fsn_lin, fsn_anglr, gt_x, gt_y,'fusion')
                # error_img = get_loss(loss, img_lin, img_anglr, gt_x, gt_y, 'img')
                error_pcl = get_loss(loss, pts/weights, gt_pts/weights, 'validation')
                error_cmd_vel = get_loss(loss, vel, gt_cmd,'validation')
                
                # error_total = error_fusion + ( 0.2 * error_img) + error_pcl

                # per_file_loss_fusion.append(error_fusion.item())
                # per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())  
                per_file_loss_pcl_cmd.append(error_cmd_vel.item())                
                # per_file_total_loss.append(error_total.item())
                            
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl'), value=np.average(per_file_loss_pcl), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl_cmd'), value=np.average(per_file_loss_pcl_cmd), epoch = epoch + 1)
            std_print(str('val_'+val_file.split('/')[-1]), 'path',np.average(per_file_loss_pcl))
            std_print(str('val_'+val_file.split('/')[-1]), 'cmd',np.average(per_file_loss_pcl_cmd))
        
            running_error.append(np.average(per_file_loss_pcl))
        
        avg_loss_on_validation = np.average(running_error)
        # print(f'epoch:------>{epoch}')
        if (epoch+1) % 10 == 0 and (epoch+1) > 20:
            print(f"saving model weights at validation error {avg_loss_on_validation}")
            torch.save({
            'model_state_dict': model.state_dict(),
            'optimizer_state_dict': optim.state_dict(),
            }, f'{model_storage_path}/unimodal_img_{epoch+1}_{avg_loss_on_validation}.pth')

        print(f'=========================> Average Validation error is:   {avg_loss_on_validation} \n')
        return avg_loss_on_validation
            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = torch.nn.L1Loss()
    model = ImageHeadMLP()
    model.to(device)
    optim = torch.optim.Adam(model.parameters(), lr = 0.0000288)     
    
    # run_validation(val_dirs, model, batch_size, 2, optim)  
    # ckpt = torch.load('/home/ranjan/Workspace/my_works/fusion-network/scripts/pre_img_way_pts_model_at_110.pth')
    # model.load_state_dict(ckpt['model_state_dict'])
    # run_validation(val_dirs, model, batch_size, 0, optim)
    # return
    # run_validation(val_dirs, model, batch_size, 0, optim)
    
    scheduler = MultiStepLR(optim, milestones= [20,70,110], gamma=.8)

    data_dict = {}
    for epoch in range(num_epochs):
        num_files = 0
        lr = scheduler.get_last_lr()        
        experiment.log_metric( name = "Learning Rate Decay", value = lr, epoch= epoch+1)
        running_loss = []
        shuffle(train_files)        
        model.train()        
        clear_dict(data_dict, epoch)
        for train_file in train_files:        
            train_loader = None 
            print(train_file)
            if train_file not in data_dict:
                train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )   
                data_dict[train_file] = train_loader
            else:
                train_loader = data_dict[train_file]

            num_files += 1
            per_file_loss_fusion = [] 
            per_file_loss_ǐmage = [] 
            per_file_loss_pcl = [] 
            per_file_loss_pcl_cmd = [] 
            per_file_total_loss = []
            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd) in enumerate(train_loader):

                # print(f'gt_cmd: {gt_cmd_vel}')
                # print(f'prev_cmd_vel:{prev_cmd_vel}')
                
                stacked_images = stacked_images.to(device)
                # pcl = pcl.to(device)
                local_goal= local_goal.to(device)                
                gt_pts= gt_pts.to(device)
                gt_cmd= gt_cmd.to(device)
                # print(f"{pcl.shape = }")
                optim.zero_grad()
                
                pts, cmd = model(stacked_images, local_goal)
                error_pcl = get_loss(loss, pts, gt_pts,'train_pcl')
                error_pcl_cmd = get_loss(loss, cmd, gt_cmd,'train_pcl')

                per_file_loss_pcl.append(error_pcl.item())     
                per_file_loss_pcl_cmd.append(error_pcl_cmd.item())   

                total_error = error_pcl + error_pcl_cmd
                
                total_error.backward()
                optim.step()

                print(f'step is:   {index} and total path error is :: {error_pcl.item()} total cmd_vel error is : {error_pcl_cmd.item()} \n')

            
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'pcl'), value=np.average(per_file_loss_pcl), epoch= epoch+1)
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'cmd'), value=np.average(per_file_loss_pcl_cmd), epoch= epoch+1)

            std_print(str(train_file.split('/')[-1]), 'path', np.average(per_file_loss_pcl))
            std_print(str(train_file.split('/')[-1]), 'cmd', np.average(per_file_loss_pcl_cmd))

            running_loss.append(np.average(per_file_loss_pcl))   
            
        scheduler.step()       
        print(f'================== epoch is: {epoch} and error is: {np.average(running_loss)}==================\n')

        if (epoch+1) % 2 == 0:
            val_error = run_validation(val_dirs, model, batch_size, epoch, optim)
            experiment.log_metric( name = "Avg Validation loss", value = np.average(val_error), epoch= epoch+1)
        # val_error_at_epoch.append(val_error)
        experiment.log_metric( name = "Avg Training loss", value = np.average(running_loss), epoch= epoch+1)
        


def main():
    train_path = "/scratch/bpanigr/fusion-network/recorded-data/train"
    # train_path = "../recorded-data/train"
    train_dirs = [ os.path.join(train_path, dir) for dir in os.listdir(train_path)]
    # validation_path = '../recorded-data/val'
    validation_path = '/scratch/bpanigr/fusion-network/recorded-data/val'
    val_dirs = [ os.path.join(validation_path, dir) for dir in os.listdir(validation_path)]


    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136021_wt')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/138181_wt')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135968_wt_at')
    # # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136514_sw_wt_sc')
    train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135967_at')


    batch_size = 90
    epochs = 250
    run_training(train_dirs, val_dirs, batch_size, epochs)



main()




scripts/run_image.py
RobotiXX/multimodal-fusion-network
from comet_ml import Experiment
import torch
import os
import numpy as np
from random import shuffle

from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from data_builder.gaussian_weights import get_gaussian_weights
from data_builder.cmd_scaler import transform_to_gt_scale
from model_builder.multimodal.fusion_net import BcFusionModel
from torch.utils.data import DataLoader
from tqdm.auto import tqdm
import matplotlib.pyplot as plt
import coloredlogs, logging
from model_builder.pcl.pcl_head import PclMLP
from model_builder.multimodal.multi_net import MultiModalNet
from torch.optim.lr_scheduler import MultiStepLR, CosineAnnealingWarmRestarts, CyclicLR

# Create an experiment with your api key
experiment = Experiment(
    #api_key="",
    # project_name= "test",
    # project_name="image-only",
    workspace="bhabaranjan",
)

experiment.add_tag('PCL-unimodal')

coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

min_val_error = 100000

val_dict = {}

model_storage_path = '/scratch/bpanigr/model_weights/unimodals'

weights = get_gaussian_weights(2,1.3)
weights = weights[:,:4] 
weights = np.concatenate([weights, weights], axis=1)
weights = torch.tensor(weights)
weights = weights.to(device)


def clear_dict(dict, epoch):
    
    if epoch % 25 == 0:
        dict.clear()

    return

def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=False, prefetch_factor=2,num_workers=8)
    return data_loader

def get_loss_prev(loss_fn, lin_vel, angular_vel, gt_lin, gt_angular, data_src):
    lin_error =  loss_fn(lin_vel, gt_lin) 
    anglr_error = loss_fn(angular_vel, gt_angular)
    error = lin_error + anglr_error
    
    lin_err_val = lin_error.item()
    anglr_error_val = anglr_error.item()

    experiment.log_metric(name = str('line_error_'+data_src), value= lin_err_val)
    experiment.log_metric(name = str('anglr_error_'+data_src), value=anglr_error_val)


    return error

def get_loss(loss_fn, pts, gt_pts, data_src):
    error =  loss_fn(pts, gt_pts)

    if data_src == 'validation':     
        experiment.log_metric(name = str('way_pts'+data_src), value= error.item())    
    else:
        experiment.log_metric(name = str('way_pts'+data_src), value=error.item())    
    return error

def std_print(file,label, value):
    print(f'{file} {label}: {value}')

def run_validation(val_files, model, batch_size, epoch, optim):
       print("Running Validation..\n")
       running_error = []
       loss = torch.nn.L1Loss()
       model.eval()
       with torch.no_grad():
        for val_file in val_files:
            
            val_loader = None
            if val_file not in val_dict:
                val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
                val_dict[val_file] = val_loader
            else:
                val_loader = val_dict[val_file]

            per_file_loss_fusion  = []
            per_file_loss_ǐmage = []
            per_file_loss_pcl = []
            per_file_loss_pcl_cmd = []
            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd) in tqdm(enumerate(val_loader)):
                pcl = pcl.to(device)
                stacked_images = stacked_images.to(device)
                local_goal= local_goal.to(device)
                
                gt_pts= gt_pts.to(device)
                gt_cmd= gt_cmd.to(device)
                
                pts, vel = model(stacked_images, pcl, local_goal)

                print("\n-------------------")
                                
                print(f'print predicted: {vel}')
                print(f'print gt_cmd: {gt_cmd}')
                              
                vel = transform_to_gt_scale(vel, device)                
                gt_cmd = transform_to_gt_scale(gt_cmd, device)  

                # vel_anglr = vel[:,0]
                # gt_anglr = gt_cmd[:,0]



                # error_angler = get_loss(loss, vel_anglr, gt_anglr,'validation')
                # print(pcl_lin)
                # print(pcl_anglr)
 
                # print(gt_x)
                # print(gt_y)
                # print('end\n')
                
                # error_fusion = get_loss(loss, fsn_lin, fsn_anglr, gt_x, gt_y,'fusion')
                # error_img = get_loss(loss, img_lin, img_anglr, gt_x, gt_y, 'img')
                error_pcl = get_loss(loss, pts/weights, gt_pts/weights, 'validation')
                error_cmd_vel = get_loss(loss, vel, gt_cmd,'validation')
                
                # error_total = error_fusion + ( 0.2 * error_img) + error_pcl

                # per_file_loss_fusion.append(error_fusion.item())
                # per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())  
                per_file_loss_pcl_cmd.append(error_cmd_vel.item())                
                # per_file_total_loss.append(error_total.item())

            print(f'avg anglr loss:{np.average(per_file_loss_pcl_cmd)} ')

            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl'), value=np.average(per_file_loss_pcl), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl_cmd'), value=np.average(per_file_loss_pcl_cmd), epoch = epoch + 1)
            std_print(str('val_'+val_file.split('/')[-1]), 'path',np.average(per_file_loss_pcl))
            std_print(str('val_'+val_file.split('/')[-1]), 'cmd',np.average(per_file_loss_pcl_cmd))
        
            running_error.append(np.average(per_file_loss_pcl))
        
        avg_loss_on_validation = np.average(running_error)
        # print(f'epoch:------>{epoch}')
        if (epoch+1) % 10 == 0 and (epoch+1) > 20:
            print(f"saving model weights at validation error {avg_loss_on_validation}")
            torch.save({
            'model_state_dict': model.state_dict(),
            'optimizer_state_dict': optim.state_dict(),
            }, f'{model_storage_path}/unimodal_pcl_{epoch+1}_{avg_loss_on_validation}.pth')

        print(f'=========================> Average Validation error is:   {avg_loss_on_validation} \n')
        return avg_loss_on_validation
            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = torch.nn.L1Loss()
    model = MultiModalNet()
    model.to(device)
    optim = torch.optim.Adam(model.parameters(), lr=0.0000098)     
    
    # run_validation(val_dirs, model, batch_size, 2, optim)  
    ckpt = torch.load('/home/ranjan/Workspace/my_works/fusion-network/scripts/tf8_end_to_end_velocities_160_0.9251317143167459.pth')
    model.load_state_dict(ckpt['model_state_dict'])
    run_validation(val_dirs, model, batch_size, 0, optim)
    return
    # run_validation(val_dirs, model, batch_size, 0, optim)
    
    scheduler = MultiStepLR(optim, milestones= [30,50,80], gamma=.8)

    data_dict = {}
    for epoch in range(num_epochs):
        num_files = 0
        lr = scheduler.get_last_lr()        
        experiment.log_metric( name = "Learning Rate Decay", value = lr, epoch= epoch+1)
        running_loss = []
        shuffle(train_files)        
        model.train()        
        clear_dict(data_dict, epoch)
        for train_file in train_files:        
            train_loader = None 
            print(train_file)
            if train_file not in data_dict:
                train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )   
                data_dict[train_file] = train_loader
            else:
                train_loader = data_dict[train_file]

            num_files += 1
            per_file_loss_fusion = [] 
            per_file_loss_ǐmage = [] 
            per_file_loss_pcl = [] 
            per_file_loss_pcl_cmd = [] 
            per_file_total_loss = []
            for index, (stacked_images, pcl ,local_goal, gt_pts, gt_cmd) in enumerate(train_loader):

                # print(f'gt_cmd: {gt_cmd_vel}')
                # print(f'prev_cmd_vel:{prev_cmd_vel}')
                
                pcl = pcl.to(device)
                stacked_images = stacked_images.to(device)
                local_goal= local_goal.to(device)                
                gt_pts= gt_pts.to(device)
                gt_cmd= gt_cmd.to(device)
                # print(f"{pcl.shape = }")
                optim.zero_grad()
                
                pts, cmd = model(stacked_images, pcl, local_goal)
                error_pcl = get_loss(loss, pts, gt_pts,'train_pcl')
                error_pcl_cmd = get_loss(loss, cmd, gt_cmd,'train_pcl')

                per_file_loss_pcl.append(error_pcl.item())     
                per_file_loss_pcl_cmd.append(error_pcl_cmd.item())   

                total_error = error_pcl + error_pcl_cmd
                
                total_error.backward()
                optim.step()

                print(f'step is:   {index} and total path error is :: {error_pcl.item()} total cmd_vel error is : {error_pcl_cmd.item()} \n')

            
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'pcl'), value=np.average(per_file_loss_pcl), epoch= epoch+1)
            experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'cmd'), value=np.average(per_file_loss_pcl_cmd), epoch= epoch+1)

            std_print(str(train_file.split('/')[-1]), 'path', np.average(per_file_loss_pcl))
            std_print(str(train_file.split('/')[-1]), 'cmd', np.average(per_file_loss_pcl_cmd))

            running_loss.append(np.average(per_file_loss_pcl))   
            
        scheduler.step()       
        print(f'================== epoch is: {epoch} and error is: {np.average(running_loss)}==================\n')

        if (epoch+1) % 2 == 0:
            val_error = run_validation(val_dirs, model, batch_size, epoch, optim)
            experiment.log_metric( name = "Avg Validation loss", value = np.average(val_error), epoch= epoch+1)
        # val_error_at_epoch.append(val_error)
        experiment.log_metric( name = "Avg Training loss", value = np.average(running_loss), epoch= epoch+1)
        


def main():
    # train_path = "/scratch/bpanigr/fusion-network/recorded-data/train"
    train_path = "../recorded-data/train"
    train_dirs = [ os.path.join(train_path, dir) for dir in os.listdir(train_path)]
    validation_path = '../recorded-data/val'
    # validation_path = '/scratch/bpanigr/fusion-network/recorded-data/val'
    val_dirs = [ os.path.join(validation_path, dir) for dir in os.listdir(validation_path)]


    # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136021_wt')
    # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/138181_wt')
    # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135968_wt_at')
    # # # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/136514_sw_wt_sc')
    # train_dirs.remove('/scratch/bpanigr/fusion-network/recorded-data/train/135967_at')


    batch_size = 1
    epochs = 250
    run_training(train_dirs, val_dirs, batch_size, epochs)



main()




scripts/run_pointnet.py
RobotiXX/multimodal-fusion-network
import time
import csv
from selenium import webdriver
from selenium.webdriver.common.by import By


wget = []
label = []
processed = []
un_processed = []

def process_link(link):
    # Configure Selenium WebDriver
    try:
        options = webdriver.ChromeOptions()
        options.add_argument('--headless')  # Run in headless mode (without opening a browser window)
        driver = webdriver.Chrome(options=options)

        # Open the link
        driver.get(link)
        time.sleep(2)  # Wait for page to load (adjust as needed)

        # Find the text inside the <code> tag
        code_element = driver.find_element(By.TAG_NAME, 'code')
        code_text = code_element.text
        print("Text inside <code> tag:", code_text)
       
        # Find the element with id "fileDescriptionBlock" ̏
        description_element = driver.find_element(By.ID, 'fileDescriptionBlock')

        # Find the <td> element within the description element and extract the text inside it
        td_element = description_element.find_element(By.TAG_NAME, 'td')
        td_text = td_element.text
        
        wget.append(code_text)
        label.append(td_text)
        processed.append(link)
        print("Text inside <td> element:", td_text)

        # Close the browser
        driver.quit()
    except:
        print(f"===>>>>could not load {link}")
        un_processed.append(link)

# Example usage
links = [
    "https://dataverse.tdl.org/file.xhtml?fileId=136510&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138181&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138164&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=133251&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136672&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138166&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=135968&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136021&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138265&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136679&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138139&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138143&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136455&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138258&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136685&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=133406&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136459&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138186&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138158&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138168&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138142&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138157&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136514&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136566&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136567&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138167&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=133228&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138173&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136453&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138263&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138260&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138262&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138141&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138144&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138153&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138146&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138147&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136498&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138184&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138174&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136500&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136515&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=135967&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136680&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138138&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138163&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136450&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136511&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138264&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136564&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138185&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138261&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136682&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138148&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136683&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136015&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136681&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138133&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138191&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136017&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138187&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=136671&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138132&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138152&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138145&version=4.0",
    "https://dataverse.tdl.org/file.xhtml?fileId=138160&version=4.0"
]

print(f"length: {len(links)}")
counter = 1
for link in links:
    print(f" counter: {counter} link: {link}")
    process_link(link)
    print("-----")
    counter += 1

# print(wget)
# print(label)

# Create a list of tuples by merging the arrays side by side
merged_data = list(zip(processed, wget, label))

# Specify the output CSV file path
output_csv_file = 'merged_data.csv'

# Write the merged data to the CSV file
with open(output_csv_file, 'w', newline='') as csvfile:
    writer = csv.writer(csvfile)
    writer.writerow(['Web Link','Download Link', 'Label'])  # Write the header row
    writer.writerows(merged_data)  # Write the merged data rows


print(un_processed)
scripts/scraper/extract_links_for_csv.py
RobotiXX/multimodal-fusion-network
import time
from selenium import webdriver
from selenium.webdriver.common.by import By

def get_links_with_text(driver, url, start_text):
    # Open the URL
    driver.get(url)
    time.sleep(2)  # Wait for page to load (adjust as needed)
    
    # Find and click the button with the specified text
    button = driver.find_element(By.XPATH, f"//span[contains(text(), '{start_text}')]")
    button.click()
    time.sleep(10)  # Wait for button action to take effect (adjust as needed)

    # Extract the links
    links = []
    elements = driver.find_elements(By.TAG_NAME, 'a')
    for element in elements:
        if element.text.startswith('A_Spot'): 
            href = element.get_attribute('href')
            links.append(href)

    return links

# Example usage
url = 'https://dataverse.tdl.org/dataset.xhtml?persistentId=doi:10.18738/T8/0PRYRH&version=4.0'
start_text = 'Tree'

# Configure Selenium WebDriver
options = webdriver.ChromeOptions()
# options.add_argument('--headless')  # Run in headless mode (without opening a browser window)
driver = webdriver.Chrome(options=options)

links = get_links_with_text(driver, url, start_text)
for link in links:
    print(link)

# Close the browser
driver.quit()

scripts/scraper/scrap_links.py
RobotiXX/multimodal-fusion-network
from comet_ml import Experiment
import torch
import os
import numpy as np
from random import shuffle

from data_builder.indexer import IndexDataset
from data_builder.transformer import ApplyTransformation
from model_builder.multimodal.fusion_net import BcFusionModel
from torch.utils.data import DataLoader
from tqdm.auto import tqdm
import matplotlib.pyplot as plt
import coloredlogs, logging

from torch.optim.lr_scheduler import MultiStepLR

# Create an experiment with your api key
experiment = Experiment(
    api_key="Ly3Tc8W7kfxPmAxsArJjX9cgo",
    # project_name= "multimodal-net-with-rnn",
    project_name="kk",
    workspace="bhabaranjan",
)

coloredlogs.install()

device = "cuda" if torch.cuda.is_available() else "cpu"

print(f'Using device ========================================>  {device}')

min_val_error = 100000

val_dict = {}

def get_data_loader(input_file_path, read_type, batch_size):
    logging.info(f'Reading {read_type} file from path {input_file_path}')
    indexer = IndexDataset(input_file_path)
    transformer = ApplyTransformation(indexer)
    data_loader = DataLoader(transformer, batch_size = batch_size, drop_last=True)
    return data_loader

def get_loss(loss_fn, lin_vel, angular_vel, gt_lin, gt_angular, data_src):
    lin_error =  loss_fn(lin_vel, gt_lin) 
    anglr_error = loss_fn(angular_vel, gt_angular)
    error = lin_error + anglr_error
    # print(f'linear error: {lin_error}')
    # print(f'angular error: {anglr_error}')

    experiment.log_metric(name = str('line_error_'+data_src), value=lin_error.item())
    experiment.log_metric(name = str('anglr_error_'+data_src), value=anglr_error.item())
    return error


def run_validation(val_files, model, batch_size, epoch, optim):
       print("Running Validation..\n")
       running_error = []
       loss = torch.nn.MSELoss()
       with torch.no_grad():
        for val_file in val_files:        
            val_loader = None
            if val_file not in val_dict:
                val_loader = get_data_loader( val_file, 'validation', batch_size = batch_size )
                val_dict[val_file] = val_loader
            else:
                val_loader = val_dict[val_file]

            per_file_loss_fusion  = []
            per_file_loss_ǐmage = []
            per_file_loss_pcl = []
            per_file_total_loss = []
            for index, (stacked_images, pcl ,local_goal, prev_cmd_vel, gt_cmd_vel) in tqdm(enumerate(val_loader)):
                stacked_images = stacked_images.to(device)
                pcl = pcl.to(device)
                local_goal= local_goal.to(device)
                prev_cmd_vel= prev_cmd_vel.to(device)
                gt_cmd_vel= gt_cmd_vel.to(device)
                
                fsn_lin, fsn_anglr, img_lin, img_anglr, pcl_lin, pcl_anglr = model(stacked_images, pcl, local_goal, prev_cmd_vel)
                

                gt_x = torch.unsqueeze(gt_cmd_vel[:,0],1)
                gt_y = torch.unsqueeze(gt_cmd_vel[:,1],1)

                print("------\n predicted")

                print(fsn_lin/100)
                print(fsn_anglr/5000)
                
                print(" gt")
                print(gt_x)
                print(gt_y)
                
                print("\n\n")
                error_fusion = get_loss(loss, fsn_lin/100, fsn_anglr/5000, gt_x, gt_y,'fusion')
                error_img = get_loss(loss, img_lin/100, img_anglr/5000, gt_x, gt_y, 'img')
                error_pcl = get_loss(loss, pcl_lin/100, pcl_anglr/5000, gt_x, gt_y, 'pcl')
                
                error_total = error_fusion + ( 0.2 * error_img) + (0.8 * error_pcl)

                per_file_loss_fusion.append(error_fusion.item())
                per_file_loss_ǐmage.append(error_img.item())
                per_file_loss_pcl.append(error_pcl.item())                
                per_file_total_loss.append(error_total.item())
                
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_img'), value=np.average(per_file_loss_ǐmage), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_pcl'), value=np.average(per_file_loss_pcl), epoch = epoch + 1)
            experiment.log_metric(name = str('val_'+val_file.split('/')[-1]+'_fusion'), value=np.average(per_file_loss_fusion), epoch = epoch + 1)

            running_error.append(np.average(per_file_loss_fusion))
            print(f'================== epoch is: {epoch} and error is: {np.average(per_file_loss_fusion)}==================\n')
        avg_loss_on_validation = np.average(running_error)
        
        if (epoch+1) % 10 == 0 and epoch!=0:
            min_val_error = avg_loss_on_validation
            print(f"saving model weights at validation error {min_val_error}")
            torch.save({
            'model_state_dict': model.state_dict(),
            'optimizer_state_dict': optim.state_dict(),
            }, f'model_at_{epoch+1}.pth')

        print(f'=========================> Average Validation error for fusion is:   {avg_loss_on_validation} \n')
        return avg_loss_on_validation
            


def run_training(train_files, val_dirs, batch_size, num_epochs):
    loss = torch.nn.MSELoss()
    model = BcFusionModel()
    ckpt = torch.load("/home/ranjan/Workspace/my_works/fusion-network/scripts/model_at_100.pth")
    model.load_state_dict(ckpt['model_state_dict'])
    # model.eval()


    optim = torch.optim.Adam(model.parameters(), lr = 0.01) 

    model.to(device)
    run_validation(val_dirs, model, batch_size, 0, optim)
    
    # model.train()
    val_error_at_epoch = []
    
    # scheduler = MultiStepLR(optim, milestones=[2,12,27,42], gamma=0.1)
    epoch_loss = []
    data_dict = {}
    # for epoch in range(num_epochs):
    #     num_files = 0
        # lr = scheduler.get_last_lr()
        # experiment.log_metric( name = "Learning Rate Decay", value = lr, epoch= epoch+1)
        # running_loss = []
        # shuffle(train_files)
        # for train_file in train_files:        
        #     train_loader = None 
        #     print(train_file)
        #     if train_file not in data_dict:
        #         train_loader = get_data_loader( train_file, 'train', batch_size = batch_size )   
        #         data_dict[train_file] = train_loader
        #     else:
        #         train_loader = data_dict[train_file]

        #     num_files += 1
        #     per_file_loss_fusion = [] 
        #     per_file_loss_ǐmage = [] 
        #     per_file_loss_pcl = [] 
        #     per_file_total_loss = []
        #     for index, (stacked_images, pcl ,local_goal, prev_cmd_vel, gt_cmd_vel) in enumerate(train_loader):

        #         # print(f'gt_cmd: {gt_cmd_vel}')
        #         # print(f'prev_cmd_vel:{prev_cmd_vel}')
                
        #         stacked_images = stacked_images.to(device)
        #         pcl = pcl.to(device)
        #         local_goal= local_goal.to(device)
        #         prev_cmd_vel= prev_cmd_vel.to(device)
        #         gt_cmd_vel= gt_cmd_vel.to(device)
        #         # print(f"{gt_cmd_vel.shape = }")
        #         optim.zero_grad()
                
        #         fsn_lin, fsn_anglr, img_lin, img_anglr, pcl_lin, pcl_anglr = model(stacked_images, pcl, local_goal, prev_cmd_vel)
                
        #         gt_x = torch.unsqueeze(gt_cmd_vel[:,0],1)
        #         gt_y = torch.unsqueeze(gt_cmd_vel[:,1],1)

        #         # print(fsn_lin)
        #         # print(fsn_anglr)

        #         # print(gt_x)
        #         # print(gt_y)
                
        #         error_fusion = get_loss(loss, fsn_lin, fsn_anglr, gt_x, gt_y,'train_fusion')
        #         error_img = get_loss(loss, img_lin, img_anglr, gt_x, gt_y, 'train_img')
        #         error_pcl = get_loss(loss, pcl_lin, pcl_anglr, gt_x, gt_y,'train_pcl')
                
        #         error_total = error_fusion + error_img + error_pcl

        #         per_file_loss_fusion.append(error_fusion.item())
        #         per_file_loss_ǐmage.append(error_img.item())
        #         per_file_loss_pcl.append(error_pcl.item())                
        #         per_file_total_loss.append(error_total.item())

        #         error_total.backward()
        #         optim.step()

        #         per_file_loss_fusion.append(error_fusion.item())
        #         per_file_loss_ǐmage.append(error_img.item())
        #         per_file_loss_pcl.append(error_pcl.item())
        #         per_file_total_loss.append(error_total.item())

        #         print(f'step is:   {index} and total error is:   {error_total.item()}  image: {error_img.item()}  pcl: {error_pcl.item()} fusion: {error_fusion.item()}\n')
            
        #     experiment.log_metric(name = str(train_file.split('/')[-1]+ " mod:" +'img'), value=np.average(per_file_loss_ǐmage), epoch= epoch+1)
        #     experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'pcl'), value=np.average(per_file_loss_pcl), epoch= epoch+1)
        #     experiment.log_metric(name = str(train_file.split('/')[-1]+" mod:" +'fusion'), value=np.average(per_file_loss_fusion), epoch= epoch+1)
        #     running_loss.append(np.average(per_file_total_loss))   
            
               
        # print(f'================== epoch is: {epoch} and error is: {np.average(running_loss)}==================\n')
        # if (epoch+1) % 2 == 0:
        #     val_error = run_validation(val_dirs, model, batch_size, epoch, optim)
        #     experiment.log_metric( name = "Avg Validation loss", value = np.average(val_error), epoch= epoch+1)
        # # val_error_at_epoch.append(val_error)
        # experiment.log_metric( name = "Avg Training loss", value = np.average(running_loss), epoch= epoch+1)
        


def main():
    train_path = "../recorded-data/train"
    # train_path = "../recorded-data/sandbox"
    train_dirs = [ os.path.join(train_path, dir) for dir in os.listdir(train_path)]
    val_dirs = [ os.path.join('../recorded-data/val', dir) for dir in os.listdir('../recorded-data/val')]
    batch_size = 2
    epochs = 250 
    run_training(train_dirs, val_dirs, batch_size, epochs)



main()




scripts/test_fusion_unscaled.py
RobotiXX/multimodal-fusion-network
import torch
from torchvision import transforms
import pickle
import numpy as np
import coloredlogs, logging
import os
import cv2
# import tf
import pyquaternion as pq

from torch.utils.data import Dataset
from scipy.spatial.transform import Rotation as R
from data_builder.transformer_pcl import get_voxelized_points
from data_builder.gaussian_weights import get_gaussian_weights

coloredlogs.install()

def read_images(path):
    # print(f"{path = }")
    image = cv2.imread(path)
    # Will have to do some re-sizing
    return cv2.cvtColor(image, cv2.COLOR_BGR2RGB)

def get_transformation_matrix(position, quaternion):
    theta = R.from_quat(quaternion).as_euler('XYZ')[2]
    robo_coordinate_in_glob_frame  = np.array([[np.cos(theta), -np.sin(theta), position[0]],
                    [np.sin(theta), np.cos(theta), position[1]],
                    [0, 0, 1]])
    return robo_coordinate_in_glob_frame

def cart2polar(xyz):
    r = np.sqrt(xyz[:, 0] ** 2 + xyz[:, 1] ** 2)
    theta =  np.arctan2(xyz[:, 1], xyz[:, 0])
    return np.stack((r,theta, xyz[:,2]), axis=1)


class ApplyTransformation(Dataset):
    def __init__(self, input_data, grid_size = [72, 30, 30]):
        self.grid_size = np.asarray(grid_size)  
        self.input_data = input_data    
        self.image_transforms = transforms.Compose([
                    transforms.ToTensor(),
                    transforms.Resize((224,224),antialias=True)
            ])
    
    def __len__(self):
         # TODO: this will return 1 example set with the following details
        return len(self.input_data)

    def __getitem__(self, _):
        self.image = self.input_data['images']
        self.point_clouds = self.input_data['pcl']
        self.way_pts = self.input_data['local_goal']        
        self.robot_position  = self.input_data['robot_pos']
        
        
        # Transform local goal into robot frame
        tf_matrix = get_transformation_matrix(self.robot_position[0],self.robot_position[1])     
        tf_inverse = np.linalg.pinv(tf_matrix)

        goals = np.concatenate([ np.array(self.way_pts), np.ones((12,1))], axis=1).transpose()

        all_pts = np.matmul(tf_inverse, goals) * get_gaussian_weights(2,1.3)
        all_pts = all_pts[:2, :]

        way_pts = all_pts[:, :4]
        local_goal = all_pts[:,4]


        point_clouds = np.array(self.point_clouds)   
        point_clouds = get_voxelized_points(point_clouds)



        local_goal = torch.tensor(local_goal, dtype=torch.float32).ravel()        

        return (point_clouds, local_goal, way_pts)

scripts/transformer.py
RobotiXX/multimodal-fusion-network