UditSinghParihar

#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/split_string.h"
#include "geometry_msgs/TransformStamped.h"

namespace cartographer_ros {

UditSinghParihar

1. Installation:
   1. Source your ROS version:
      1. source /opt/ros/kinetic/setup.bash
   2. Create temporary catkin workspace or if you want to do in existing workspace, then skip this step:
      1. mkdir -p ~/p3dx_ws/src
      2. cd ~/p3dx_ws/ && catkin_make
      3. source devel/setup.bash
   3. Download p3dx and teleop ros packages:
      1. cd ~/p3dx_ws/src
      2. git clone https://github.com/allenh1/p2os.git

UditSinghParihar

#!/usr/bin/env python

import rospy
from nav_msgs.msg import OccupancyGrid, MapMetaData
import tf2_ros
import numpy as np

def occToNumpy(msg):
	data = np.asarray(msg.data, dtype=np.int8).reshape(msg.info.height, msg.info.width)
	return data

UditSinghParihar

1. Below are the manhattan graphs generated on both less noisy and more noisy unoptimized trajectory.
2. Both manhattan graphs from less and more noisy trajectories looks same, so we could do loop closure detection using MLP on both.

UditSinghParihar

1. Rectified poses with correct thetas
2. Manhattan constraints on dense graph less noisy trajectory in g2o format
3. Constraints on different sections:
   I: del_x = 0.1 m; del_y = 0.5 m; del_theta = 0 degree
   II: del_x = 0.1 m; del_y = 0.3 m; del_theta = 0 degree
   III: del_x = 0.1 m; del_y = 0.1 m; del_theta = 270 degree
4. Performing g2o optimization on the graph is causing trajectory to go haywire.

UditSinghParihar

1. Manhattan constraints results on dataset 2.
2. Due to smooth robot trajectories, the final manhattan map generated on the whole dataset seems pretty close to ground truth.
3. This could be good input for MLP and graph optimization can be done using both Manhattan world constraints and loop closing constraints on dense graph in one shot.

UditSinghParihar

1. Nomenclature of mlp_out_dense.txt.
2. node 1 and node 2 are matched nodes from MLP.
3. Coloumn notation( all ids corresponds to dense graph indexes):
   1. 1st col: Starting dense index of node 1.
   2. 2nd col: Ending dense index of node 1.
   3. 3rd col: Starting dense index of node 2.
4. 4th col: Ending dense index of node 2.

UditSinghParihar

1. ICP's resutls comparison.
2. The results with ICP constraints(along with some manhattan constraints) seems to giving close results compared to just optimizing with manhattan constraints.

UditSinghParihar

1. Following are the results of dataset 2 optimization just using manhattan constraints.

UditSinghParihar

1. Following are the results of noisy star shaped trajectory optimization just using manhattan constraints.