From b17f9d781b53c32c8e81fe112c0f064f0db08aeb Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Tue, 29 Mar 2022 22:35:32 +0530 Subject: [PATCH 1/8] Testing --- Dockerfile | 51 ++++++++ depth_slam.py | 332 ++++++++++++++++++++++++++++++++++++++++++++++++++ frame.py | 3 +- slam.py | 13 +- 4 files changed, 397 insertions(+), 2 deletions(-) create mode 100644 Dockerfile create mode 100755 depth_slam.py diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..f622c49 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,51 @@ +#FROM python +FROM ubuntu:20.04 +RUN mkdir /app +WORKDIR /app +ARG DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y python3-pip +RUN apt-get install ffmpeg libsm6 libxext6 -y +RUN apt-get install -y libeigen3-dev +RUN apt-get install -y git +RUN /usr/bin/python3 -m pip install opencv-python +RUN /usr/bin/python3 -m pip install pygame + +# Install CMake +#RUN apt install -y libprotobuf-dev protobuf-compiler +RUN apt-get install -y cmake +RUN apt-get install -y g++ +RUN apt-get install -y freeglut3 +RUN apt-get install -y freeglut3-dev +RUN apt-get install -y libglew-dev +RUN apt-get install -y binutils-gold + +# Install Eigen +WORKDIR /app +RUN git clone https://gitlab.com/libeigen/eigen.git +WORKDIR /app/eigen +RUN git checkout 3.3.4 +RUN mkdir build +WORKDIR /app/eigen/build +RUN /usr/bin/cmake .. +RUN make install + +# Install pangolin +WORKDIR /app +#RUN git clone https://github.com/uoip/pangolin.git +RUN git clone https://github.com/AdityaNG/pangolin.git +WORKDIR /app/pangolin +RUN mkdir build +WORKDIR /app/pangolin/build +RUN /usr/bin/cmake .. +#RUN /usr/bin/make -j8 +RUN /usr/bin/make +WORKDIR /app/pangolin/ +RUN ls pangolin*.so +RUN rm pangolin.cpython-38-x86_64-linux-gnu.so +RUN /usr/bin/python3 setup.py install + +# Install twitchslam +WORKDIR /app +RUN git clone https://github.com/geohot/twitchslam +WORKDIR /app/twitchslam +CMD /usr/bin/python3 slam.py \ No newline at end of file diff --git a/depth_slam.py b/depth_slam.py new file mode 100755 index 0000000..de7bf94 --- /dev/null +++ b/depth_slam.py @@ -0,0 +1,332 @@ +#!/usr/bin/env python3 + +# 1644664247513 + +import os +import sys + +sys.path.append("lib/macosx") +sys.path.append("lib/linux") + +import time +import cv2 +from display import Display2D, Display3D +from frame import Frame, match_frames +import numpy as np +import g2o +from pointmap import Map, Point +from helpers import triangulate, add_ones + +np.set_printoptions(suppress=True) + +class SLAM(object): + def __init__(self, W, H, K): + # main classes + self.mapp = Map() + + # params + self.W, self.H = W, H + self.K = K + + def process_frame(self, img, pose=None, verts=None): + start_time = time.time() + assert img.shape[0:2] == (self.H, self.W) + frame = Frame(self.mapp, img, self.K, verts=verts) + + if frame.id == 0: + return + + f1 = self.mapp.frames[-1] + f2 = self.mapp.frames[-2] + + idx1, idx2, Rt = match_frames(f1, f2) + + # add new observations if the point is already observed in the previous frame + # TODO: consider tradeoff doing this before/after search by projection + for i,idx in enumerate(idx2): + if f2.pts[idx] is not None and f1.pts[idx1[i]] is None: + f2.pts[idx].add_observation(f1, idx1[i]) + + if frame.id < 5 or True: + # get initial positions from fundamental matrix + f1.pose = np.dot(Rt, f2.pose) + else: + # kinematic model (not used) + velocity = np.dot(f2.pose, np.linalg.inv(self.mapp.frames[-3].pose)) + f1.pose = np.dot(velocity, f2.pose) + + # pose optimization + if pose is None: + #print(f1.pose) + pose_opt = self.mapp.optimize(local_window=1, fix_points=True) + print("Pose: %f" % pose_opt) + #print(f1.pose) + else: + # have ground truth for pose + f1.pose = pose + + sbp_pts_count = 0 + + # search by projection + if len(self.mapp.points) > 0: + # project *all* the map points into the current frame + map_points = np.array([p.homogeneous() for p in self.mapp.points]) + projs = np.dot(np.dot(self.K, f1.pose[:3]), map_points.T).T + projs = projs[:, 0:2] / projs[:, 2:] + + # only the points that fit in the frame + good_pts = (projs[:, 0] > 0) & (projs[:, 0] < self.W) & \ + (projs[:, 1] > 0) & (projs[:, 1] < self.H) + + for i, p in enumerate(self.mapp.points): + if not good_pts[i]: + # point not visible in frame + continue + if f1 in p.frames: + # we already matched this map point to this frame + # TODO: understand this better + continue + for m_idx in f1.kd.query_ball_point(projs[i], 2): + # if point unmatched + if f1.pts[m_idx] is None: + b_dist = p.orb_distance(f1.des[m_idx]) + # if any descriptors within 64 + if b_dist < 64.0: + p.add_observation(f1, m_idx) + sbp_pts_count += 1 + break + + # triangulate the points we don't have matches for + good_pts4d = np.array([f1.pts[i] is None for i in idx1]) + + # do triangulation in global frame + pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2]) + good_pts4d &= np.abs(pts4d[:, 3]) != 0 + pts4d /= pts4d[:, 3:] # homogeneous 3-D coords + + # adding new points to the map from pairwise matches + new_pts_count = 0 + for i,p in enumerate(pts4d): + if not good_pts4d[i]: + continue + + # check parallax is large enough + # TODO: learn what parallax means + """ + r1 = np.dot(f1.pose[:3, :3], add_ones(f1.kps[idx1[i]])) + r2 = np.dot(f2.pose[:3, :3], add_ones(f2.kps[idx2[i]])) + parallax = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2)) + if parallax >= 0.9998: + continue + """ + + # check points are in front of both cameras + pl1 = np.dot(f1.pose, p) + pl2 = np.dot(f2.pose, p) + if pl1[2] < 0 or pl2[2] < 0: + continue + + # reproject + pp1 = np.dot(self.K, pl1[:3]) + pp2 = np.dot(self.K, pl2[:3]) + + # check reprojection error + pp1 = (pp1[0:2] / pp1[2]) - f1.kpus[idx1[i]] + pp2 = (pp2[0:2] / pp2[2]) - f2.kpus[idx2[i]] + pp1 = np.sum(pp1**2) + pp2 = np.sum(pp2**2) + if pp1 > 2 or pp2 > 2: + continue + + # add the point + try: + color = img[int(round(f1.kpus[idx1[i],1])), int(round(f1.kpus[idx1[i],0]))] + except IndexError: + color = (255,0,0) + pt = Point(self.mapp, p[0:3], color) + pt.add_observation(f2, idx2[i]) + pt.add_observation(f1, idx1[i]) + new_pts_count += 1 + + print("Adding: %d new points, %d search by projection" % (new_pts_count, sbp_pts_count)) + + # optimize the map + if frame.id >= 4 and frame.id%5 == 0: + err = self.mapp.optimize() #verbose=True) + print("Optimize: %f units of error" % err) + + print("Map: %d points, %d frames" % (len(self.mapp.points), len(self.mapp.frames))) + print("Time: %.2f ms" % ((time.time()-start_time)*1000.0)) + print(np.linalg.inv(f1.pose)) + + +import os +#import cv2 +import numpy as np +from skimage.metrics import structural_similarity as compare_ssim +import matplotlib.pyplot as plt + +import pandas as pd +import signal +from multiprocessing import Process, Array, Pool, Queue +import ctypes +import glob +import time +import cv2 + +#manydepth_path = "dataset/manydepth/000008" +dataset_dir = "/mnt/HDD/home/aditya/elab_visualiser/elab" + +#data = sorted(glob.glob(dataset_dir + "/*")) +data = sorted(glob.glob(dataset_dir + "/1644664247513")) + +def image_loop(): + + #FRAME_SKIP = int(os.getenv("FSKIP")) + for folder_path in data[-1:]: + id = folder_path.split("/")[-1] + start_time = int(id) + CSV_path = os.path.join(folder_path, id + ".csv") + + csv_dat = pd.read_csv(CSV_path) + frames_folder_path = os.path.join(folder_path, "frames") + depth_folder_path = os.path.join(folder_path, 'depth_midas') + #frames_folder_path = os.path.join(folder_path, "inputs_midas") + #depth_folder_path = os.path.join(folder_path, 'outputs_midas') + + frames_list = sorted(glob.glob(frames_folder_path + "/*")) + + #for frame_path in frames_list[::FRAME_SKIP]: + for frame_path in frames_list: + frame_name = frame_path.split("/")[-1] + depth_file_path = os.path.join(depth_folder_path, frame_name) + + Timestamp_frame = float(".".join(frame_path.split("/")[-1].split(".")[:-1])) + print(depth_file_path) + #frame = PIL.Image.open(frame_path) + #depth = PIL.Image.open(depth_file_path) + frame = cv2.imread(frame_path) + depth = cv2.imread(depth_file_path) + depth = cv2.cvtColor(depth, cv2.COLOR_BGR2GRAY) + #height, width = depth.shape + #scale_factor = 0.5 + #frame = cv2.resize(frame, (int(width*scale_factor), int(height*scale_factor))) + #depth = cv2.resize(depth, (int(width*scale_factor), int(height*scale_factor))) + + + + print(Timestamp_frame) + + sensors_data = csv_dat[csv_dat['Timestamp'] == Timestamp_frame] + if len(sensors_data): + #Timestamp, Longitude, Latitude, RotationVX, RotationVY,RotationVZ,RotationVW,RotationVAcc,linear_acc_x,linear_acc_y,linear_acc_z,heading,speed = sensors_data.values[0] + sen_data = sensors_data.values[0] + + + #yield frame, depth, list(sen_data) + yield frame, depth, [] + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("%s " % sys.argv[0]) + exit(-1) + + disp2d, disp3d = None, None + + if os.getenv("HEADLESS") is None: + disp3d = Display3D() + + cap = cv2.VideoCapture(sys.argv[1]) + + # camera parameters + W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + + CNT = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + F = float(os.getenv("F", "525")) + if os.getenv("SEEK") is not None: + cap.set(cv2.CAP_PROP_POS_FRAMES, int(os.getenv("SEEK"))) + + if W > 1024: + downscale = 1024.0/W + F *= downscale + H = int(H * downscale) + W = 1024 + print("using camera %dx%d with F %f" % (W,H,F)) + + # camera intrinsics + K = np.array([[F,0,W//2],[0,F,H//2],[0,0,1]]) + Kinv = np.linalg.inv(K) + + # create 2-D display + if os.getenv("HEADLESS") is None: + disp2d = Display2D(W, H) + + slam = SLAM(W, H, K) + + """ + mapp.deserialize(open('map.json').read()) + while 1: + disp3d.paint(mapp) + time.sleep(1) + """ + + gt_pose = None + if len(sys.argv) >= 3: + gt_pose = np.load(sys.argv[2])['pose'] + # add scale param? + gt_pose[:, :3, 3] *= 50 + + i = 0 + #while cap.isOpened(): + while True: + + """ + frame_old, depth_old, sen_data_old = image_loop() + frame, depth, sen_data = frame_old, depth_old, sen_data_old + + FRAME_SKIP = int(os.getenv("FSKIP")) + i=0 + while i Date: Tue, 29 Mar 2022 23:57:14 +0530 Subject: [PATCH 2/8] Docker install working --- Dockerfile | 27 ++++++++++++++++++++++----- slam.py | 6 ++++-- 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/Dockerfile b/Dockerfile index f622c49..de798c5 100644 --- a/Dockerfile +++ b/Dockerfile @@ -9,6 +9,7 @@ RUN apt-get install -y libeigen3-dev RUN apt-get install -y git RUN /usr/bin/python3 -m pip install opencv-python RUN /usr/bin/python3 -m pip install pygame +RUN /usr/bin/python3 -m pip install PyOpenGL # Install CMake #RUN apt install -y libprotobuf-dev protobuf-compiler @@ -18,6 +19,9 @@ RUN apt-get install -y freeglut3 RUN apt-get install -y freeglut3-dev RUN apt-get install -y libglew-dev RUN apt-get install -y binutils-gold +RUN apt-get install -y libsuitesparse-dev +RUN /usr/bin/python3 -m pip install scipy +RUN /usr/bin/python3 -m pip install scikit-image # Install Eigen WORKDIR /app @@ -27,6 +31,7 @@ RUN git checkout 3.3.4 RUN mkdir build WORKDIR /app/eigen/build RUN /usr/bin/cmake .. +RUN /usr/bin/make -j12 RUN make install # Install pangolin @@ -37,15 +42,27 @@ WORKDIR /app/pangolin RUN mkdir build WORKDIR /app/pangolin/build RUN /usr/bin/cmake .. -#RUN /usr/bin/make -j8 -RUN /usr/bin/make +RUN /usr/bin/make -j12 +#RUN /usr/bin/make WORKDIR /app/pangolin/ -RUN ls pangolin*.so -RUN rm pangolin.cpython-38-x86_64-linux-gnu.so +RUN ls pangolin*.so +RUN /usr/bin/python3 setup.py install + +# Install g2opy +WORKDIR /app +RUN git clone https://github.com/uoip/g2opy.git +WORKDIR /app/g2opy +RUN mkdir build +WORKDIR /app/g2opy/build +RUN /usr/bin/cmake .. +RUN /usr/bin/make -j12 +RUN /usr/bin/make install +WORKDIR /app/g2opy/ RUN /usr/bin/python3 setup.py install # Install twitchslam WORKDIR /app -RUN git clone https://github.com/geohot/twitchslam +#RUN git clone https://github.com/geohot/twitchslam +RUN git clone https://github.com/AdityaNG/twitchslam WORKDIR /app/twitchslam CMD /usr/bin/python3 slam.py \ No newline at end of file diff --git a/slam.py b/slam.py index 8b098a7..049e257 100755 --- a/slam.py +++ b/slam.py @@ -5,8 +5,10 @@ import os import sys -sys.path.append("lib/macosx") -sys.path.append("lib/linux") +#sys.path.append("lib/macosx") +#sys.path.append("lib/linux") + +#Path(os.getcwd()).parent.absolute() import time import cv2 From 29c46158274e581f3f61897a28df21fce068e36d Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Sun, 3 Apr 2022 20:33:19 +0530 Subject: [PATCH 3/8] Code working, no GUI --- Dockerfile | 15 +++++++++++++-- README.md | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ run.sh | 2 ++ 3 files changed, 65 insertions(+), 2 deletions(-) create mode 100755 run.sh diff --git a/Dockerfile b/Dockerfile index de798c5..99c824a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,6 @@ #FROM python -FROM ubuntu:20.04 +#FROM ubuntu:20.04 +FROM nvidia/vulkan:1.1.121 RUN mkdir /app WORKDIR /app ARG DEBIAN_FRONTEND=noninteractive @@ -7,9 +8,11 @@ RUN apt-get update && apt-get install -y python3-pip RUN apt-get install ffmpeg libsm6 libxext6 -y RUN apt-get install -y libeigen3-dev RUN apt-get install -y git +RUN /usr/bin/python3 -m pip install --upgrade pip RUN /usr/bin/python3 -m pip install opencv-python RUN /usr/bin/python3 -m pip install pygame RUN /usr/bin/python3 -m pip install PyOpenGL +RUN /usr/bin/python3 -m pip install scikit-build # Install CMake #RUN apt install -y libprotobuf-dev protobuf-compiler @@ -65,4 +68,12 @@ WORKDIR /app #RUN git clone https://github.com/geohot/twitchslam RUN git clone https://github.com/AdityaNG/twitchslam WORKDIR /app/twitchslam -CMD /usr/bin/python3 slam.py \ No newline at end of file +RUN git checkout docker + +RUN touch /root/.Xauthority +RUN apt-get -y install xauth + +EXPOSE 8887 +WORKDIR /app/twitchslam +#CMD /usr/bin/python3 slam.py +CMD sh run.sh \ No newline at end of file diff --git a/README.md b/README.md index 77df6db..c5a47b6 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,54 @@ A toy implementation of monocular SLAM written while livestreaming. +# Docker +```bash +docker build -t twitchslam . +docker run --runtime=nvidia -it -d --gpus all --net=host \ + -e DISPLAY -v /tmp/.X11-unix \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e SEEK=100 -e FSKIP=5 -e F=1000 + --env DISPLAY_COOKIE="5ed98dd8a299505cac6bc6004209b381" \ + twitchslam + +docker run --runtime=nvidia -it -d --gpus all --net=host \ + -e DISPLAY -v /tmp/.X11-unix \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + --env DISPLAY_COOKIE="(DISPLAY_COOKIE)" \ + twitchslam \ + /bin/sh /fsds/run.sh + +docker run --runtime=nvidia -it --gpus all --net=host \ + -e DISPLAY -v /tmp/.X11-unix \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e SEEK=100 -e FSKIP=5 -e F=1000 \ + --env DISPLAY_COOKIE="5ed98dd8a299505cac6bc6004209b381" \ + twitchslam /bin/bash + +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam /bin/bash + +sudo apt install nvidia-docker2 + +sudo systemctl daemon-reload +sudo systemctl restart docker + +touch .Xauthority +xauth add $DISPLAY_COOKIE + +F=525 python3 slam.py videos/test_freiburgxyz525.mp4 + +# https://stackoverflow.com/questions/15933493/pygame-error-no-available-video-device +FSKIP=1 SDL_VIDEODRIVER="dummy" F=525 python3 slam.py videos/test_freiburgxyz525.mp4 + + +``` + +You’ll need to replace DISPLAY_COOKIE with your your display cookie which you can get from xauth, it should look like the following +```bash +$ xauth list +username/machine:0 MIT-MAGIC-COOKIE-1 [32 character string] +``` + Stream ----- @@ -25,6 +73,8 @@ export F=500 # Focal length (in px) # good example F=525 ./slam.py videos/test_freiburgxyz525.mp4 +F=525 python3 slam.py videos/test_freiburgxyz525.mp4 + # ground truth F=525 ./slam.py videos/test_freiburgrpy525.mp4 videos/test_freiburgrpy525.npz diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..b087fc3 --- /dev/null +++ b/run.sh @@ -0,0 +1,2 @@ +xauth add $DISPLAY_COOKIE +FSKIP=1 SDL_VIDEODRIVER="dummy" F=525 python3 slam.py videos/test_freiburgxyz525.mp4 \ No newline at end of file From df1d88d07cecb185592a0739029f88054afd518b Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Sun, 3 Apr 2022 20:36:13 +0530 Subject: [PATCH 4/8] Documentation --- Dockerfile | 2 +- README.md | 36 ++++-------------------------------- 2 files changed, 5 insertions(+), 33 deletions(-) diff --git a/Dockerfile b/Dockerfile index 99c824a..0dee320 100644 --- a/Dockerfile +++ b/Dockerfile @@ -66,7 +66,7 @@ RUN /usr/bin/python3 setup.py install # Install twitchslam WORKDIR /app #RUN git clone https://github.com/geohot/twitchslam -RUN git clone https://github.com/AdityaNG/twitchslam +RUN git clone https://github.com/AdityaNG/twitchslam WORKDIR /app/twitchslam RUN git checkout docker diff --git a/README.md b/README.md index c5a47b6..25aa42c 100644 --- a/README.md +++ b/README.md @@ -7,43 +7,15 @@ A toy implementation of monocular SLAM written while livestreaming. # Docker ```bash docker build -t twitchslam . -docker run --runtime=nvidia -it -d --gpus all --net=host \ - -e DISPLAY -v /tmp/.X11-unix \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - -e SEEK=100 -e FSKIP=5 -e F=1000 - --env DISPLAY_COOKIE="5ed98dd8a299505cac6bc6004209b381" \ - twitchslam - -docker run --runtime=nvidia -it -d --gpus all --net=host \ - -e DISPLAY -v /tmp/.X11-unix \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - --env DISPLAY_COOKIE="(DISPLAY_COOKIE)" \ - twitchslam \ - /bin/sh /fsds/run.sh - -docker run --runtime=nvidia -it --gpus all --net=host \ - -e DISPLAY -v /tmp/.X11-unix \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - -e SEEK=100 -e FSKIP=5 -e F=1000 \ - --env DISPLAY_COOKIE="5ed98dd8a299505cac6bc6004209b381" \ - twitchslam /bin/bash - -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam /bin/bash +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam +``` +## Install nvidia-docker2 +```bash sudo apt install nvidia-docker2 sudo systemctl daemon-reload sudo systemctl restart docker - -touch .Xauthority -xauth add $DISPLAY_COOKIE - -F=525 python3 slam.py videos/test_freiburgxyz525.mp4 - -# https://stackoverflow.com/questions/15933493/pygame-error-no-available-video-device -FSKIP=1 SDL_VIDEODRIVER="dummy" F=525 python3 slam.py videos/test_freiburgxyz525.mp4 - - ``` You’ll need to replace DISPLAY_COOKIE with your your display cookie which you can get from xauth, it should look like the following From 8369c2895c3e3dcbafa39e2631d3d7c1ad555453 Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Sun, 3 Apr 2022 20:49:50 +0530 Subject: [PATCH 5/8] GUI working --- README.md | 4 ++++ run.sh | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 25aa42c..874a4cd 100644 --- a/README.md +++ b/README.md @@ -8,10 +8,14 @@ A toy implementation of monocular SLAM written while livestreaming. ```bash docker build -t twitchslam . docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam + +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam /bin/bash ``` ## Install nvidia-docker2 ```bash +xhost +local:docker + sudo apt install nvidia-docker2 sudo systemctl daemon-reload diff --git a/run.sh b/run.sh index b087fc3..1047cf1 100755 --- a/run.sh +++ b/run.sh @@ -1,2 +1,2 @@ xauth add $DISPLAY_COOKIE -FSKIP=1 SDL_VIDEODRIVER="dummy" F=525 python3 slam.py videos/test_freiburgxyz525.mp4 \ No newline at end of file +FSKIP=1 F=525 python3 slam.py videos/test_freiburgxyz525.mp4 \ No newline at end of file From c3a78690e6113509bd6cf2275e65945a71d06f85 Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Mon, 4 Apr 2022 10:14:14 +0530 Subject: [PATCH 6/8] Passing videos through volume mounting --- Dockerfile | 5 ++++- README.md | 47 +++++++++++++++++++++++++++++++++++++++-------- depth_slam.py | 2 ++ run.sh | 3 ++- slam.py | 4 +++- 5 files changed, 50 insertions(+), 11 deletions(-) diff --git a/Dockerfile b/Dockerfile index 0dee320..456b860 100644 --- a/Dockerfile +++ b/Dockerfile @@ -63,10 +63,13 @@ RUN /usr/bin/make install WORKDIR /app/g2opy/ RUN /usr/bin/python3 setup.py install +RUN /usr/bin/python3 -m pip install pandas + # Install twitchslam WORKDIR /app #RUN git clone https://github.com/geohot/twitchslam -RUN git clone https://github.com/AdityaNG/twitchslam +#RUN git clone https://github.com/AdityaNG/twitchslam +COPY . /app/twitchslam WORKDIR /app/twitchslam RUN git checkout docker diff --git a/README.md b/README.md index 874a4cd..84bafbc 100644 --- a/README.md +++ b/README.md @@ -7,12 +7,49 @@ A toy implementation of monocular SLAM written while livestreaming. # Docker ```bash docker build -t twitchslam . -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="sped-machine/unix:0 MIT-MAGIC-COOKIE-1 5ed98dd8a299505cac6bc6004209b381" -it twitchslam /bin/bash +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="DISPLAY_COOKIE" -it twitchslam +``` + +You’ll need to replace DISPLAY_COOKIE with your your display cookie which you can get from xauth, it should look like the following +```bash +$ xauth list +username/machine:0 MIT-MAGIC-COOKIE-1 [32 character string] +``` +The final command should look something like one of the following +```bash +docker run --runtime=nvidia --gpus all --net=host \ + -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ + --mount type=bind,source=/mnt/HDD/home/aditya/elab_visualiser/elab/1644664247513,target=/videos \ + --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ + --env VIDEO_PATH="1644664247513.mp4" \ + -e SEEK=100 -e FSKIP=5 -e F=1000 \ + -it twitchslam + +docker run --runtime=nvidia --gpus all --net=host \ + -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ + --mount type=bind,source="$(pwd)"/videos,target=/videos \ + --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ + --env VIDEO_PATH="test_freiburgrpy525.mp4" \ + -e F=525 \ + -it twitchslam + +docker run --runtime=nvidia --gpus all --net=host \ + -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ + --mount type=bind,source="$(pwd)"/videos,target=/videos \ + --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ + --env VIDEO_PATH="test_freiburgxyz525.mp4" \ + -e F=525 \ + -it twitchslam + +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --mount type=bind,source="$(pwd)"/videos,target=/videos --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" --env VIDEO_PATH="test_freiburgrpy525.mp4" -it twitchslam + +# To get shell access +docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --mount type=bind,source="$(pwd)"/videos,target=/videos --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" -it twitchslam /bin/bash ``` ## Install nvidia-docker2 + ```bash xhost +local:docker @@ -22,12 +59,6 @@ sudo systemctl daemon-reload sudo systemctl restart docker ``` -You’ll need to replace DISPLAY_COOKIE with your your display cookie which you can get from xauth, it should look like the following -```bash -$ xauth list -username/machine:0 MIT-MAGIC-COOKIE-1 [32 character string] -``` - Stream ----- diff --git a/depth_slam.py b/depth_slam.py index de7bf94..1acf65b 100755 --- a/depth_slam.py +++ b/depth_slam.py @@ -174,6 +174,8 @@ def process_frame(self, img, pose=None, verts=None): import time import cv2 +FRAME_SKIP = 1 if not os.getenv("FSKIP") else int(os.getenv("FSKIP")) + #manydepth_path = "dataset/manydepth/000008" dataset_dir = "/mnt/HDD/home/aditya/elab_visualiser/elab" diff --git a/run.sh b/run.sh index 1047cf1..12e854a 100755 --- a/run.sh +++ b/run.sh @@ -1,2 +1,3 @@ xauth add $DISPLAY_COOKIE -FSKIP=1 F=525 python3 slam.py videos/test_freiburgxyz525.mp4 \ No newline at end of file +#FSKIP=1 F=525 python3 slam.py /videos/test_freiburgxyz525.mp4 +python3 slam.py /videos/$VIDEO_PATH \ No newline at end of file diff --git a/slam.py b/slam.py index 049e257..43b2fce 100755 --- a/slam.py +++ b/slam.py @@ -12,6 +12,7 @@ import time import cv2 +from depth_slam import FRAME_SKIP from display import Display2D, Display3D from frame import Frame, match_frames import numpy as np @@ -217,7 +218,8 @@ def process_frame(self, img, pose=None, verts=None): while cap.isOpened(): ret, frame_old = cap.read() - FRAME_SKIP = int(os.getenv("FSKIP")) + #FRAME_SKIP = int(os.getenv("FSKIP")) +1 + FRAME_SKIP = 1 if not os.getenv("FSKIP") else int(os.getenv("FSKIP")) i=0 while ret and i Date: Mon, 16 May 2022 10:13:02 +0530 Subject: [PATCH 7/8] DockerHub and a run script --- README.md | 42 +++++++----------------------------------- twitchslam_docker.sh | 12 ++++++++++++ 2 files changed, 19 insertions(+), 35 deletions(-) create mode 100755 twitchslam_docker.sh diff --git a/README.md b/README.md index 84bafbc..ca4d39d 100644 --- a/README.md +++ b/README.md @@ -5,47 +5,19 @@ A toy implementation of monocular SLAM written while livestreaming. # Docker +Pull the docker container ```bash -docker build -t twitchslam . - -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --env DISPLAY_COOKIE="DISPLAY_COOKIE" -it twitchslam +docker pull adityang5/twitchslam ``` -You’ll need to replace DISPLAY_COOKIE with your your display cookie which you can get from xauth, it should look like the following +Or you can build the container using the following ```bash -$ xauth list -username/machine:0 MIT-MAGIC-COOKIE-1 [32 character string] +docker build -t twitchslam . ``` -The final command should look something like one of the following + +You can run the container customizing the provided `twitchslam_docker.sh` ```bash -docker run --runtime=nvidia --gpus all --net=host \ - -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ - --mount type=bind,source=/mnt/HDD/home/aditya/elab_visualiser/elab/1644664247513,target=/videos \ - --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ - --env VIDEO_PATH="1644664247513.mp4" \ - -e SEEK=100 -e FSKIP=5 -e F=1000 \ - -it twitchslam - -docker run --runtime=nvidia --gpus all --net=host \ - -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ - --mount type=bind,source="$(pwd)"/videos,target=/videos \ - --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ - --env VIDEO_PATH="test_freiburgrpy525.mp4" \ - -e F=525 \ - -it twitchslam - -docker run --runtime=nvidia --gpus all --net=host \ - -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ - --mount type=bind,source="$(pwd)"/videos,target=/videos \ - --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" \ - --env VIDEO_PATH="test_freiburgxyz525.mp4" \ - -e F=525 \ - -it twitchslam - -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --mount type=bind,source="$(pwd)"/videos,target=/videos --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" --env VIDEO_PATH="test_freiburgrpy525.mp4" -it twitchslam - -# To get shell access -docker run --runtime=nvidia --gpus all --net=host -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all --mount type=bind,source="$(pwd)"/videos,target=/videos --env DISPLAY_COOKIE="sped-machine/unix:1 MIT-MAGIC-COOKIE-1 a1654666c7a832aa599d9cf267033379" -it twitchslam /bin/bash +./twitchslam_docker.sh ``` ## Install nvidia-docker2 diff --git a/twitchslam_docker.sh b/twitchslam_docker.sh new file mode 100755 index 0000000..7f0be96 --- /dev/null +++ b/twitchslam_docker.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +xhost +local:docker +a=$(xauth list | head -n 1) + +docker run --runtime=nvidia --gpus all --net=host \ + -e DISPLAY --rm -v /tmp/.X11-unix -e NVIDIA_DRIVER_CAPABILITIES=all \ + --mount type=bind,source=/home/aditya/VSProjects/twitchslam/videos,target=/videos \ + --env DISPLAY_COOKIE="$a" \ + --env VIDEO_PATH="test_drone.mp4" \ + -e SEEK=100 -e FSKIP=5 -e F=1000 \ + -it adityang5/twitchslam \ No newline at end of file From 9fcf05b94e8c625c7862b1702822276a29b94b79 Mon Sep 17 00:00:00 2001 From: AdityaNG Date: Mon, 16 May 2022 10:24:26 +0530 Subject: [PATCH 8/8] Clean up --- depth_slam.py | 334 -------------------------------------------------- slam.py | 9 -- 2 files changed, 343 deletions(-) delete mode 100755 depth_slam.py diff --git a/depth_slam.py b/depth_slam.py deleted file mode 100755 index 1acf65b..0000000 --- a/depth_slam.py +++ /dev/null @@ -1,334 +0,0 @@ -#!/usr/bin/env python3 - -# 1644664247513 - -import os -import sys - -sys.path.append("lib/macosx") -sys.path.append("lib/linux") - -import time -import cv2 -from display import Display2D, Display3D -from frame import Frame, match_frames -import numpy as np -import g2o -from pointmap import Map, Point -from helpers import triangulate, add_ones - -np.set_printoptions(suppress=True) - -class SLAM(object): - def __init__(self, W, H, K): - # main classes - self.mapp = Map() - - # params - self.W, self.H = W, H - self.K = K - - def process_frame(self, img, pose=None, verts=None): - start_time = time.time() - assert img.shape[0:2] == (self.H, self.W) - frame = Frame(self.mapp, img, self.K, verts=verts) - - if frame.id == 0: - return - - f1 = self.mapp.frames[-1] - f2 = self.mapp.frames[-2] - - idx1, idx2, Rt = match_frames(f1, f2) - - # add new observations if the point is already observed in the previous frame - # TODO: consider tradeoff doing this before/after search by projection - for i,idx in enumerate(idx2): - if f2.pts[idx] is not None and f1.pts[idx1[i]] is None: - f2.pts[idx].add_observation(f1, idx1[i]) - - if frame.id < 5 or True: - # get initial positions from fundamental matrix - f1.pose = np.dot(Rt, f2.pose) - else: - # kinematic model (not used) - velocity = np.dot(f2.pose, np.linalg.inv(self.mapp.frames[-3].pose)) - f1.pose = np.dot(velocity, f2.pose) - - # pose optimization - if pose is None: - #print(f1.pose) - pose_opt = self.mapp.optimize(local_window=1, fix_points=True) - print("Pose: %f" % pose_opt) - #print(f1.pose) - else: - # have ground truth for pose - f1.pose = pose - - sbp_pts_count = 0 - - # search by projection - if len(self.mapp.points) > 0: - # project *all* the map points into the current frame - map_points = np.array([p.homogeneous() for p in self.mapp.points]) - projs = np.dot(np.dot(self.K, f1.pose[:3]), map_points.T).T - projs = projs[:, 0:2] / projs[:, 2:] - - # only the points that fit in the frame - good_pts = (projs[:, 0] > 0) & (projs[:, 0] < self.W) & \ - (projs[:, 1] > 0) & (projs[:, 1] < self.H) - - for i, p in enumerate(self.mapp.points): - if not good_pts[i]: - # point not visible in frame - continue - if f1 in p.frames: - # we already matched this map point to this frame - # TODO: understand this better - continue - for m_idx in f1.kd.query_ball_point(projs[i], 2): - # if point unmatched - if f1.pts[m_idx] is None: - b_dist = p.orb_distance(f1.des[m_idx]) - # if any descriptors within 64 - if b_dist < 64.0: - p.add_observation(f1, m_idx) - sbp_pts_count += 1 - break - - # triangulate the points we don't have matches for - good_pts4d = np.array([f1.pts[i] is None for i in idx1]) - - # do triangulation in global frame - pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2]) - good_pts4d &= np.abs(pts4d[:, 3]) != 0 - pts4d /= pts4d[:, 3:] # homogeneous 3-D coords - - # adding new points to the map from pairwise matches - new_pts_count = 0 - for i,p in enumerate(pts4d): - if not good_pts4d[i]: - continue - - # check parallax is large enough - # TODO: learn what parallax means - """ - r1 = np.dot(f1.pose[:3, :3], add_ones(f1.kps[idx1[i]])) - r2 = np.dot(f2.pose[:3, :3], add_ones(f2.kps[idx2[i]])) - parallax = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2)) - if parallax >= 0.9998: - continue - """ - - # check points are in front of both cameras - pl1 = np.dot(f1.pose, p) - pl2 = np.dot(f2.pose, p) - if pl1[2] < 0 or pl2[2] < 0: - continue - - # reproject - pp1 = np.dot(self.K, pl1[:3]) - pp2 = np.dot(self.K, pl2[:3]) - - # check reprojection error - pp1 = (pp1[0:2] / pp1[2]) - f1.kpus[idx1[i]] - pp2 = (pp2[0:2] / pp2[2]) - f2.kpus[idx2[i]] - pp1 = np.sum(pp1**2) - pp2 = np.sum(pp2**2) - if pp1 > 2 or pp2 > 2: - continue - - # add the point - try: - color = img[int(round(f1.kpus[idx1[i],1])), int(round(f1.kpus[idx1[i],0]))] - except IndexError: - color = (255,0,0) - pt = Point(self.mapp, p[0:3], color) - pt.add_observation(f2, idx2[i]) - pt.add_observation(f1, idx1[i]) - new_pts_count += 1 - - print("Adding: %d new points, %d search by projection" % (new_pts_count, sbp_pts_count)) - - # optimize the map - if frame.id >= 4 and frame.id%5 == 0: - err = self.mapp.optimize() #verbose=True) - print("Optimize: %f units of error" % err) - - print("Map: %d points, %d frames" % (len(self.mapp.points), len(self.mapp.frames))) - print("Time: %.2f ms" % ((time.time()-start_time)*1000.0)) - print(np.linalg.inv(f1.pose)) - - -import os -#import cv2 -import numpy as np -from skimage.metrics import structural_similarity as compare_ssim -import matplotlib.pyplot as plt - -import pandas as pd -import signal -from multiprocessing import Process, Array, Pool, Queue -import ctypes -import glob -import time -import cv2 - -FRAME_SKIP = 1 if not os.getenv("FSKIP") else int(os.getenv("FSKIP")) - -#manydepth_path = "dataset/manydepth/000008" -dataset_dir = "/mnt/HDD/home/aditya/elab_visualiser/elab" - -#data = sorted(glob.glob(dataset_dir + "/*")) -data = sorted(glob.glob(dataset_dir + "/1644664247513")) - -def image_loop(): - - #FRAME_SKIP = int(os.getenv("FSKIP")) - for folder_path in data[-1:]: - id = folder_path.split("/")[-1] - start_time = int(id) - CSV_path = os.path.join(folder_path, id + ".csv") - - csv_dat = pd.read_csv(CSV_path) - frames_folder_path = os.path.join(folder_path, "frames") - depth_folder_path = os.path.join(folder_path, 'depth_midas') - #frames_folder_path = os.path.join(folder_path, "inputs_midas") - #depth_folder_path = os.path.join(folder_path, 'outputs_midas') - - frames_list = sorted(glob.glob(frames_folder_path + "/*")) - - #for frame_path in frames_list[::FRAME_SKIP]: - for frame_path in frames_list: - frame_name = frame_path.split("/")[-1] - depth_file_path = os.path.join(depth_folder_path, frame_name) - - Timestamp_frame = float(".".join(frame_path.split("/")[-1].split(".")[:-1])) - print(depth_file_path) - #frame = PIL.Image.open(frame_path) - #depth = PIL.Image.open(depth_file_path) - frame = cv2.imread(frame_path) - depth = cv2.imread(depth_file_path) - depth = cv2.cvtColor(depth, cv2.COLOR_BGR2GRAY) - #height, width = depth.shape - #scale_factor = 0.5 - #frame = cv2.resize(frame, (int(width*scale_factor), int(height*scale_factor))) - #depth = cv2.resize(depth, (int(width*scale_factor), int(height*scale_factor))) - - - - print(Timestamp_frame) - - sensors_data = csv_dat[csv_dat['Timestamp'] == Timestamp_frame] - if len(sensors_data): - #Timestamp, Longitude, Latitude, RotationVX, RotationVY,RotationVZ,RotationVW,RotationVAcc,linear_acc_x,linear_acc_y,linear_acc_z,heading,speed = sensors_data.values[0] - sen_data = sensors_data.values[0] - - - #yield frame, depth, list(sen_data) - yield frame, depth, [] - -if __name__ == "__main__": - if len(sys.argv) < 2: - print("%s " % sys.argv[0]) - exit(-1) - - disp2d, disp3d = None, None - - if os.getenv("HEADLESS") is None: - disp3d = Display3D() - - cap = cv2.VideoCapture(sys.argv[1]) - - # camera parameters - W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) - H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) - - CNT = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) - F = float(os.getenv("F", "525")) - if os.getenv("SEEK") is not None: - cap.set(cv2.CAP_PROP_POS_FRAMES, int(os.getenv("SEEK"))) - - if W > 1024: - downscale = 1024.0/W - F *= downscale - H = int(H * downscale) - W = 1024 - print("using camera %dx%d with F %f" % (W,H,F)) - - # camera intrinsics - K = np.array([[F,0,W//2],[0,F,H//2],[0,0,1]]) - Kinv = np.linalg.inv(K) - - # create 2-D display - if os.getenv("HEADLESS") is None: - disp2d = Display2D(W, H) - - slam = SLAM(W, H, K) - - """ - mapp.deserialize(open('map.json').read()) - while 1: - disp3d.paint(mapp) - time.sleep(1) - """ - - gt_pose = None - if len(sys.argv) >= 3: - gt_pose = np.load(sys.argv[2])['pose'] - # add scale param? - gt_pose[:, :3, 3] *= 50 - - i = 0 - #while cap.isOpened(): - while True: - - """ - frame_old, depth_old, sen_data_old = image_loop() - frame, depth, sen_data = frame_old, depth_old, sen_data_old - - FRAME_SKIP = int(os.getenv("FSKIP")) - i=0 - while i