快速安装

源码可在https://github.com/learnmoreonce/SLAM 下载

安装十分简单,参照官方指南,使用以下命令即可。

# Install the required libraries that are available as debs.
sudo apt-get update
sudo apt-get install -y \\
    cmake \\
    g++ \\
    git \\
    google-mock \\
    libboost-all-dev \\
    libcairo2-dev \\
    libeigen3-dev \\
    libgflags-dev \\
    libgoogle-glog-dev \\
    liblua5.2-dev \\
    libprotobuf-dev \\
    libsuitesparse-dev \\
    libwebp-dev \\
    ninja-build \\
    protobuf-compiler \\
    python-sphinx

# Build and install Ceres.
git clone <https://ceres-solver.googlesource.com/ceres-solver>
cd ceres-solver
mkdir build
cd build
cmake .. -G Ninja
ninja
ninja test
sudo ninja install

# Build and install Cartographer.
cd cartographer
mkdir build
cd build
cmake .. -G Ninja
ninja
ninja test
sudo ninja install
 
# Install wstool and rosdep.
sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

# Create a new workspace in 'catkin_ws'.
mkdir catkin_ws
cd catkin_ws
wstool init src

# Merge the cartographer_ros.rosinstall file and fetch code for dependencies.
wstool merge -t src <https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall>
wstool update -t src

# Install deb dependencies.
# The command 'sudo rosdep init' will print an error if you have already
# executed it since installing ROS. This error can be ignored.
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

# Build and install.
catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

# Download the 2D backpack example bag.
wget -P ~/Downloads <https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag>

运行demo
# Launch the 2D backpack demo.
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

重要文件,百度云已有,可直接使用百度云离线下载:

cartographer_paper_revo_lds.bag         3M

taurob_tracker_simulation.bag           42.4M

2011-09-15-08-32-46.bag                 3.66G

b3-2016-04-05-14-14-00.bag              8.43G

cartographer_paper_deutsches_museum.bag 470.5M

Cartographer 现已经支持 Toyota HSR、TurtleBots、PR2、Revo LDS 这几个机器人平台。

common-port.h

源码分析文件:common/port.h

/*
  common-port.h文件主要实现2大功能:
  1,使用std::lround对浮点数进行四舍五入取整运算
  2,利用boost的iostreams/filter/gzip对字符串压缩与解压缩
*/

#ifndef CARTOGRAPHER_COMMON_PORT_H_
#define CARTOGRAPHER_COMMON_PORT_H_

#include <cinttypes>
#include <cmath>
#include <string>

#include <boost/iostreams/device/back_inserter.hpp>
#include <boost/iostreams/filter/gzip.hpp>     //包含多种解压与压缩算法
#include <boost/iostreams/filtering_stream.hpp>//配合filter实现流过滤

using int8 = int8_t;
using int16 = int16_t;
using int32 = int32_t;
using int64 = int64_t;
using uint8 = uint8_t;
using uint16 = uint16_t;
using uint32 = uint32_t;
using uint64 = uint64_t;

using std::string;
namespace cartographer {
namespace common {

/*
lround(+2.3) = 2  lround(+2.5) = 3  lround(+2.7) = 3
lround(-2.3) = -2  lround(-2.5) = -3  lround(-2.7) = -3
lround(-0.0) = 0
*/
inline int RoundToInt(const float x) { return std::lround(x); }//四舍五入取整运算

inline int RoundToInt(const double x) { return std::lround(x); }

inline int64 RoundToInt64(const float x) { return std::lround(x); }

inline int64 RoundToInt64(const double x) { return std::lround(x); }

/*
利用gzip_compressor对string进行压缩,第一参数是未压缩string,第二参数是完成压缩string
*/
inline void FastGzipString(const string& uncompressed, string* compressed) {
  boost::iostreams::filtering_ostream out;//创建过滤流
  out.push(
      boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));//使用快速压缩算法
  out.push(boost::iostreams::back_inserter(*compressed));//对compressed 使用后插迭代器
  boost::iostreams::write(out,
                          reinterpret_cast<const char*>(uncompressed.data()),
                          uncompressed.size());//压缩 char *,插入compressed
}

/*
利用gzip_decompressor解压缩string,第一参数是待解压的string,第二参数是解压后的string
*/
inline void FastGunzipString(const string& compressed, string* decompressed) {
  boost::iostreams::filtering_ostream out;        //创建过滤流
  out.push(boost::iostreams::gzip_decompressor());//指定解压缩算法
  out.push(boost::iostreams::back_inserter(*decompressed));//对decompressed使用后插入迭代器
  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
                          compressed.size());//解压缩char*,插入decompressed
}

}  // namespace common
}  // namespace cartographer

#endif  // CARTOGRAPHER_COMMON_PORT_H_

/*

c++11 已经支持back_inserter。
std::back_inserter执行push_back操作, 返回值back_insert_iterator, 并实现自增.
具体用法可见 port_le_test.cpp
*/

/*
reinterpret_cast的转换格式:reinterpret_cast <type-id> (expression)
允许将任何指针类型转换为指定的指针类型
*/

/*
boost::iostreams/filtering_ostream用法:
 
压缩时
filtering_ostream out;
out.push(gzip_compressor()); //gzip OutputFilter
out.push(bzip2_compressor());//bzip2 OutputFilter
out.push(boost::iostreams::file_sink("test.txt"));//以file_sink device结束
将流的数据先按gzip压缩,然后再按bzip2压缩之后,才输出到text.txt文件中。

解压时
filtering_istream in;
in.push(gzip_decompressor());/gzip InputFilter
in.push(bzip2_decompressor());/bzip2 InputFilter
in.push(file_source("test.txt"));
先将test.txt文件中数据读出,然后按bzip2解压,然后再按gzip解压,存入in流中,正好是压缩的逆序。
*/

ref:

使用boost::iostreams库压缩和解压数据_伯诚的Blog-CSDN博客

common-time.h

源码分析文件:common/time.h

/*
预备知识:
c++11 提供了语言级别的时间函数.包括duration和time_point
duration是时间段,指的是某单位时间上的一个明确的tick(片刻数):
3分钟->"3个1分钟",
1.5个"1/3秒" :1.5是tick,1/3秒是时间单位

time_point是一个duration和一个epoch(起点)的组合:
2017年5月4日是"自1970,01,01"以来的126200000秒数

common/time.h主要功能是提供时间转换函数:

 */

#ifndef CARTOGRAPHER_COMMON_TIME_H_
#define CARTOGRAPHER_COMMON_TIME_H_

#include <chrono>
#include <ostream>
#include <ratio>

#include "cartographer/common/port.h"

namespace cartographer {
namespace common {

//719162 是0001年1月1日到1970年1月1日所经历的天数
constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds =
    (719162ll * 24ll * 60ll * 60ll);

struct UniversalTimeScaleClock {
  using rep = int64;
  using period = std::ratio<1, 10000000>; //百万分之一秒,1us 》错了,应该是0.1us.
  //以下涉及到us的均应该纠正为0.1us
  
  using duration = std::chrono::duration<rep, period>;
  using time_point = std::chrono::time_point<UniversalTimeScaleClock>;
  /*time_point的模板参数是UniversalTimeScaleClock,
  那为何其可以做模板参数呢:?符合std::关于clock的类型定义和static成员*/
  static constexpr bool is_steady = true;
};

// Represents Universal Time Scale durations and timestamps which are 64-bit
// integers representing the 100 nanosecond ticks since the Epoch which is
// January 1, 1 at the start of day in UTC.
using Duration = UniversalTimeScaleClock::duration;//微秒,us
using Time = UniversalTimeScaleClock::time_point;  //时间点

/*Time::min()是chrono自带的函数。返回一个低于1970.01.01的数。

编译运行cpp/cppstdlib_2nd/util/chrono1.cpp:
epoch: Thu Jan  1 08:00:00 1970
now:   Tue Jul  4 19:39:29 2017
min:   Tue Sep 21 08:18:27 1677
max:   Sat Apr 12 07:47:16 2262

*/
// Convenience functions to create common::Durations.
//将秒数seconds转为c++的duration实例对象
Duration FromSeconds(double seconds);              
Duration FromMilliseconds(int64 milliseconds);     //毫秒

// Returns the given duration in seconds.
//将的duration实例对象转为 秒数 
double ToSeconds(Duration duration);               

// Creates a time from a Universal Time Scale.     
//将TUC时间(微秒)转化为c++的time_point对象
Time FromUniversal(int64 ticks);

// Outputs the Universal Time Scale timestamp for a given Time.
//将c++的time_point对象转为TUC时间,单位是us
int64 ToUniversal(Time time);                      

// For logging and unit tests, outputs the timestamp integer.
//重载<<操作符,将time_point以string输出
std::ostream& operator<<(std::ostream& os, Time time); 

}  // namespace common
}  // namespace cartographer

#endif  // CARTOGRAPHER_COMMON_TIME_H_

/*
linux下关于time转换:
<http://blog.chinaunix.net/uid-20532339-id-1931780.html>
<https://stackoverflow.com/questions/2883576/how-do-you-convert-epoch-time-in-c/7844741#7844741>
<https://www.epochconverter.com/batch*/>

.

common/time.cc

#include "cartographer/common/time.h"

#include <string>

namespace cartographer {
namespace common {

//duration_cast是c++ 11的时间显式转换函数.
Duration FromSeconds(const double seconds) {
  return std::chrono::duration_cast<Duration>(
    //将double类型的秒数转化为duration对象
      std::chrono::duration<double>(seconds));  
}

double ToSeconds(const Duration duration) {
//反转化,count()返回时钟周期数,ticks
  return std::chrono::duration_cast<std::chrono::duration<double>>(duration)
      .count();
}

//先构造一个临时duration对象,再将其转化为time_point对象
//Duration(ticks)调用的是UniversalTimeScaleClock的构造函数     
Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); } 

//count()返回time_point自epoch以来的时钟周期数
int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); }

//先将Time转化为 int64 , 再转为字符串形式
std::ostream& operator<<(std::ostream& os, const Time time) {
  os << std::to_string(ToUniversal(time));
  return os;
}

//mill是ms,micro是us,先将毫秒转为以毫秒计时的duration对象,再转化为以微妙计.
common::Duration FromMilliseconds(const int64 milliseconds) {
  return std::chrono::duration_cast<Duration>(
      std::chrono::milliseconds(milliseconds));
}

}  // namespace common
}  // namespace cartographer