This repository contains the packages responsible for performing global traversability mapping and can be very easily integrated with any SLAM system.
 |
 |
|---|---|
| Traversability Map generated with the 3D PointCloud and SLAM | Gazebo world of the map |
git clone https://github.com/suchetanrs/traversability_mapping
Run the following to execute a shell script that installs all the dependencies and build the library.
cd <cloned repository>
./build.shThe parameter file for the library is in traversability_mapping/params
In this, you need to provide the transform between the lidar frame and camera (SLAM) frame. NOTE: In case you do not intend to provide a LiDAR pointcloud and wish to send your own pointcloud in camera frame, set this transform to 0
You can include this library in the CMakeLists.txt of your SLAM by doing the following.
Here we assume your SLAM system is named as ${PROJECT_NAME}. Change this as you desire.
A compile flag -DWITH_TRAVERSABILITY_MAP is added so that you can enable or disable this library while running your SLAM system.
find_package(traversability_mapping)
if(traversability_mapping_FOUND)
include_directories(${PROJECT_NAME}
${traversability_mapping_INCLUDE_DIRS}
)
ADD_DEFINITIONS("-DWITH_TRAVERSABILITY_MAP")
endif(traversability_mapping_FOUND)
if(traversability_mapping_FOUND)
target_link_libraries(${PROJECT_NAME}
type_conversion
traversabilitySystem
)
endif(traversability_mapping_FOUND)#ifdef WITH_TRAVERSABILITY_MAP
#include "traversability_mapping/System.hpp"
#endifNOTE: THERE SHOULD BE ONLY ONE INSTANCE OF THIS CLASS CREATED
You don't need to worry about running this instance in a seperate thread since it is handled internally by the library.
In case you need access to the traversability class pointer across multiple classes in your SLAM system, pass the newly created pointer to the constructors of the class that needs access. Do not create a new instance for it :p
// private class member
traversability_mapping::System* pTraversability_;
// new instance
pTraversability_ = new traversability_mapping::System();NOTE: Only one map needs to be created for a single traversability instance. You can create multiple maps in case the SLAM handles loss of tracking by creating a new map.
long unsigned int mapid = 0;
pTraversability_->addNewLocalMap(mapid);The library provides a buffer to store the 3D pointclouds. In case you initialize a KeyFrame without the 3D Pointcloud, it handles it by assigning the Pointcloud with the closest timestamp to the keyframe. It is better to do this in the PointCloud callback.
NOTE: This needs to be done continously
// pcl is of type sensor_msgs::msg::PointCloud2
pTraversability_->pushToBuffer(pcl); Once a map is created, you can add keyframes to the map to generate traversability. This is done by giving a keyframe ID along with the timestamp and an already created map ID.
The timestamps can be passed either as a double value of the type seconds.nanoseconds or an unsigned long long value of only nanoseconds.
The keyFrame ID (kfID) must be a non-negative integer. This automatically matches the Keyframe with the PointCloud having the closest timestamp in the buffer.
pTraversability_->addNewKeyFrame(timestamp, kFID, mapid);In case you also want to assign a pointcloud directly to the keyframe instead of using the Pointclouds in the buffer, you can do it by passing a pointcloud of the type sensor_msgs::msg::PointCloud2
pTraversability_->addNewKeyFrame(timestamp, kFID, mapid, pointCloud);The poses can be passed either as Eigen::Affine3d or Sophus::SE3f.
NOTE: The keyframe pose must be in the map frame.
// Eigen::Affine3d pose;
pTraversability_->updateKeyFrame(kFID, pose);These poses can be updated continously during optimization and loop closing.
You can do this after every keyframe update step.
There is an individual occupancy map for every local Map you create. The following lines will return the occupancy map of the currently active Local Map (That is, the map to which you are adding new keyframes.)
if(pTraversability_->getLocalMap() != nullptr)
{
auto OccupancyGrid = pTraversability_->getLocalMap()->getOccupancyMap();
}You can do this after every keyframe update step.
if(pTraversability_->getLocalMap() != nullptr)
{
auto gridmap = pTraversability_->getLocalMap()->getGridMap();
}
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent