BLAMをインストールには、Intel MKLとGTSAMをインストールしてからBLAMをインストールします。
依存ライブラリをインストール
sudo apt-get install libtbb-dev libboost-all-dev cmake cpio python-wstool
Intel MKLをインストール
installing using APTに従えばよい.
$ cd /tmp
$ wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB
$ sudo apt-key add GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB
$ sudo wget https://apt.repos.intel.com/setup/intelproducts.list -O /etc/apt/sources.list.d/intelproducts.list
$ sudo apt-get update
$ sudo apt install intel-mkl-2020.0-088
GTSAMをインストール
Quickstartに従えばよい.
$ git clone https://bitbucket.org/gtborg/gtsam.git
$ cd gtsam
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ sudo make install
BLAMをインストール
$ git clone https://github.com/naisy/blam.git
$ cd blam
$ ./build.sh
BLAMを実行
onlineモード
$ ./blam_online.sh
offlineモード
# rosbagに /velodyne_points のTopicが存在している前提
# internal/src/blam_example/launch/test_offline.launchでTopic名変更可能
$ ./blam_offline.sh rosbag_file_path
BLAM実行エラーの解消
エラー
internal/devel/lib/blam_slam/blam_slam_offline: error while loading shared libraries: libmetis.so: cannot open shared object file: No such file or directory
- 解消方法
~/.bashrcのLD_LIBRARY_PATHに/usr/local/libパスを追加
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
エラー
Assertion `key_arg.x <= this->max_key_.x' failed.
blam_slam_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/octree/include/pcl/octree/impl/octree_pointcloud.hpp:688: void pcl::octree::OctreePointCloud::genOctreeKeyforPoint(const PointT&, pcl::octree::OctreeKey&) const [with PointT = pcl::PointXYZ; LeafContainerT = pcl::octree::OctreeContainerPointIndices; BranchContainerT = pcl::octree::OctreeContainerEmpty; OctreeT = pcl::octree::OctreeBase]: Assertion `key_arg.x <= this->max_key_.x' failed.
- 解消方法
PointCloudMapper.cc
change
if (!map_octree_->isVoxelOccupiedAtPoint(p)) {
to:
double min_x, min_y, min_z, max_x, max_y, max_z;
map_octree_->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
if (!isInBox || !map_octree_->isVoxelOccupiedAtPoint(p)) {