Graph slam impl

.gitignore

@@ -30,3 +30,7 @@
 *.exe
 *.out
 *.app
+
+# rosbags and g2o files
+/data/g2o/*.g2o
+/data/rosbag/*.bag

GraphSLAM/bag2graph.ipynb

@@ -0,0 +1,182 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 12,
+   "id": "4a7fa10c",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import pcl\n",
+    "import rosbag\n",
+    "import numpy as np\n",
+    "import gtsam as gt\n",
+    "import ros_numpy\n",
+    "import sensor_msgs\n",
+    "import time\n",
+    "from tqdm import tqdm"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 13,
+   "id": "eea4933c",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "bag = rosbag.Bag('./data/2022-04-04-18-44-50.bag')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 14,
+   "id": "b270ecb8",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "vertex = {} # {time: [i, gt.Pose2]}\n",
+    "i = int(0)\n",
+    "for vertex_str, vertex_msg, vertex_time in bag.read_messages(topics=['/vertex_odom']):\n",
+    "    temp = vertex_msg.data.split(' ')\n",
+    "    t,x,y,theta = np.array(temp[1:]).astype(np.float64)\n",
+    "    temp_pose = gt.Pose2(x, y, theta)\n",
+    "    vertex[t] = [i,temp_pose]\n",
+    "    i = int(i+1)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 15,
+   "id": "62e97dee",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "edges = [] # i,j,x,y,theta,noiseModel\n",
+    "for edge_str, edge_msg, edge_time in bag.read_messages(topics=['/edge_odom']):\n",
+    "    data = edge_msg.data.split(' ')\n",
+    "    st,ct,x,y,theta = np.array(data[1:6]).astype(np.float64)\n",
+    "    q11, q12, q13, q22, q23, q33 = np.array(data[6:]).astype(np.float64)\n",
+    "    info = np.array([q11, q12, q13, q12, q22, q23, q13, q23, q33]).reshape([3,3])\n",
+    "    noiseModel = gt.noiseModel.Gaussian.Information(info)\n",
+    "    if vertex.get(st) is not None and vertex.get(ct) is not None:\n",
+    "        i = vertex.get(st)[0]\n",
+    "        j = vertex.get(ct)[0]\n",
+    "        edges.append([i,j,x,y,theta,noiseModel])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 16,
+   "id": "7ce2bf51",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def convert2PCL(pc_msg):\n",
+    "    pc_msg.__class__ = sensor_msgs.msg._PointCloud2.PointCloud2\n",
+    "    offset_sorted = {f.offset: f for f in pc_msg.fields}\n",
+    "    pc_msg.fields = [f for (_, f) in sorted(offset_sorted.items())]\n",
+    "    pc_np = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pc_msg, remove_nans=True)\n",
+    "    pc_pcl = pcl.PointCloud(np.array(pc_np, dtype=np.float32))\n",
+    "    return pc_pcl"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 17,
+   "id": "0c5005cb",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "pc_dict = {} # {id: ros::pc2}\n",
+    "for pc_str, pc_msg, pc_time in bag.read_messages(topics=['/currentFeatureInWorld']):\n",
+    "    ct = pc_msg.header.stamp.to_sec()\n",
+    "    idx = vertex.get(ct, vertex[min(vertex.keys(), key=lambda k: abs(k-ct))])[0]  #Find closest index\n",
+    "    pc_dict[idx] = convert2PCL(pc_msg)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 19,
+   "id": "0e110b13",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "  0%|          | 1/2879 [00:00<00:06, 468.90it/s]\n",
+      "  0%|          | 0/2879 [00:00<?, ?it/s]\n"
+     ]
+    },
+    {
+     "ename": "ValueError",
+     "evalue": "order must be one of 'C', 'F', 'A', or 'K' (got 'fortran')",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mValueError\u001b[0m                                Traceback (most recent call last)",
+      "\u001b[1;32m/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb Cell 7'\u001b[0m in \u001b[0;36m<cell line: 4>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb#ch0000006?line=7'>8</a>\u001b[0m \u001b[39mfor\u001b[39;00m i, targetCloud \u001b[39min\u001b[39;00m tqdm(pc_dict\u001b[39m.\u001b[39mitems()):\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb#ch0000006?line=8'>9</a>\u001b[0m     \u001b[39mif\u001b[39;00m i \u001b[39m!=\u001b[39m idx:\n\u001b[0;32m---> <a href='vscode-notebook-cell:/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb#ch0000006?line=9'>10</a>\u001b[0m         boolCoverge, T, estimate, fitnessScore \u001b[39m=\u001b[39m icp\u001b[39m.\u001b[39;49micp(inputCloud, targetCloud)\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb#ch0000006?line=10'>11</a>\u001b[0m         matchMatrix\u001b[39m.\u001b[39mappend([i,fitnessScore,T])\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/hjhaveri/Documents/rob530_finalproject/AVP-SLAM-PLUS/GraphSLAM/bag2graph.ipynb#ch0000006?line=11'>12</a>\u001b[0m matchMatrix \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray(matchMatrix)\n",
+      "File \u001b[0;32mpcl/pxi/registration/IterativeClosestPoint_180.pxi:105\u001b[0m, in \u001b[0;36mpcl._pcl.IterativeClosestPoint.icp\u001b[0;34m()\u001b[0m\n",
+      "File \u001b[0;32mpcl/pxi/registration/IterativeClosestPoint_180.pxi:72\u001b[0m, in \u001b[0;36mpcl._pcl.IterativeClosestPoint.run\u001b[0;34m()\u001b[0m\n",
+      "\u001b[0;31mValueError\u001b[0m: order must be one of 'C', 'F', 'A', or 'K' (got 'fortran')"
+     ]
+    }
+   ],
+   "source": [
+    "#https://github.com/strawlab/python-pcl/blob/master/examples/official/Registration/iterative_closest_point.py\n",
+    "fitnessThreshold =1.8\n",
+    "interEdges = [] # [i,j,T] \n",
+    "for idx, inputCloud in tqdm(pc_dict.items()):\n",
+    "    icp = inputCloud.make_IterativeClosestPoint()\n",
+    "    matchMatrix = [] #[i,fitnessScore,T]\n",
+    "    for i, targetCloud in tqdm(pc_dict.items()):\n",
+    "        if i != idx:\n",
+    "            boolCoverge, T, estimate, fitnessScore = icp.icp(inputCloud, targetCloud)\n",
+    "            matchMatrix.append([i,fitnessScore,T])\n",
+    "    matchMatrix = np.array(matchMatrix)\n",
+    "    bestMatches = matchMatrix[matchMatrix[:,1]>fitnessThreshold]\n",
+    "    for bM in bestMatches:\n",
+    "        interEdges.append([idx, bM[0], bM[2]]) # [i,j,T]   "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "d011f984",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "np.savez('processedData.npz', vertex=**vertex, edges=np.array(edges), interEdges=np.array(interEdges))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "a0332989",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.10"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

avp_slam_plus/CMakeLists.txt

@@ -1,10 +1,12 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(avp_slam_plus)
 
+set(CMAKE_SOURCE_DIR "/home/devin/catkin_ws/src")
 set(CMAKE_BUILD_TYPE "Release")
 set(CMAKE_CXX_FLAGS "-std=c++11")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 
+
 find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   nav_msgs
@@ -45,4 +47,12 @@ add_executable(mapping src/mapping.cpp)
 target_link_libraries(mapping ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
 
 add_executable(localization src/localization.cpp)
-target_link_libraries(localization ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
+target_link_libraries(localization ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
+
+catkin_install_python(PROGRAMS scripts/Odometry.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS scripts/talker.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

avp_slam_plus/config/configFile.yaml

@@ -22,7 +22,7 @@ farPointThresh: 20
 closePointThresh: 0.1
 
 mapSave: true
-mapSaveLocation: "/home/lgt/catkin_ws/src/AVP-SLAM-PLUS/avp_slam_plus/data/laserCloudMap.pcd"
+mapSaveLocation: "/home/catkin_ws/src/AVP-SLAM-PLUS/avp_slam_plus/data/laserCloudMap.pcd"
 odomKeyFramDisThresh: 1.0
 InvalidColorThresh: 60
 useICP: false
@@ -37,4 +37,3 @@ ndtResolution: 1.0
 ndtFitnessScoreThresh: 0.3
 
 
-