pre和next|C++C++ 模板与STL

std::prev

原型template <class BidirectionalIterator> BidirectionalIterator prev (BidirectionalIterator it, typename iterator_traits<BidirectionalIterator>::difference_type n = 1);

该函数会返回一个距离 it 迭代器 n 个元素的新迭代器。当 n 为正数时，其返回的迭代器将位于 it 左侧；反之，当 n 为负数时，其返回的迭代器位于 it 右侧。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#include <iostream>     // std::cout
#include <iterator>     // std::next
#include <list>         // std::list
using namespace std;
int main() {
    //创建并初始化一个 list 容器
    std::list<int> mylist{ 1,2,3,4,5 };
    std::list<int>::iterator it = mylist.end();
    //获取一个距离 it 迭代器 2 个元素的迭代器，由于 2 为正数，newit 位于 it 左侧
    auto newit = prev(it, 2);
    cout << "prev(it, 2) = " << *newit << endl;   // 4

    //n为负数，newit 位于 it 右侧
    it = mylist.begin();
    newit = prev(it, -2);
    cout << "prev(it, -2) = " << *newit;   // 3
    return 0;
}

prev() 函数自身不会检验新迭代器的指向是否合理，需要我们自己来保证其合理性。

next()函数和prev()函数恰好相反

pbstream_main的使用|激光SLAMCartographer原理和配置

旧版本为cartographer_pbstream

源码是pbstream_main.cc，又实际调用了pbstream_info.cc 和 pbstream_migrate.cc

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
int main(int argc, char** argv)
{
  google::InitGoogleLogging(argv[0]);

  FLAGS_logtostderr = true;
  const std::string usage_message =
      "Swiss Army knife for pbstreams.\n\n"
      "Currently supported subcommands are:\n"
      "\tinfo    - Prints summary of pbstream.\n"
      "\tmigrate - Migrates old pbstream (w/o header) to new pbstream format.";
  google::ParseCommandLineFlags(&argc, &argv, true);

  if (argc < 2) {
    google::SetUsageMessage(usage_message);
    google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
    return EXIT_FAILURE;
  } else if (std::string(argv[1]) == "info") {
    return ::cartographer::io::pbstream_info(argc, argv);
  } else if (std::string(argv[1]) == "migrate") {
    return ::cartographer::io::pbstream_migrate(argc, argv);
  }
  // 其他情况会报错
}

可执行文件在carto_ws/install_isolated/bin

pbstream_main info

终端执行pbstream_main info，调用pbstream_info：打印出pbstream文件的概况，路径支持环境变量。

1
./pbstream_main info $MAPS/file.pbstream

结果的中间一大部分是采用的lua参数，如果不想显示这些参数，注释掉源码里的LOG(INFO) << "Trajectory options: " << trajectory_options.DebugString();

结果如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
Reading pbstream file from '/home/xiaoqiang/carto_ws/maps/file.pbstream'...
Header: format_version: 2
Trajectory options: sensor_id {
  id: "scan"
}
sensor_id {
  type: IMU
  id: "imu"
}
sensor_id {
  type: ODOMETRY
  id: "odom"
}

# ......省略lua参数
Trajectory id: 0 has #nodes 9778 has #submaps 161
SerializedData package contains #node: 9778
SerializedData package contains #odometry_data: 49697

SerializedData package contains #submap: 161
SerializedData package contains #submap_2d: 161
SerializedData package contains #submap_2d_grid: 161

SerializedData package contains #submap_3d: 0
SerializedData package contains #submap_3d_high_resolution_hybrid_grid: 0
SerializedData package contains #trajectory_data: 1

不要在最后加-all_debug_strings=true，否则打印出无穷无尽的内容，包括了栅格值和子图转换表等等。

pbstream_main migrate

终端执行pbstream_main migrate，调用pbstream_migrate：把pbstream文件移植到新的子图格式。把未使用histogram的子图文件移植为包含histogram的子图格式。如果想在输出文件中排除未完成的子图，设置--include_unfinished_submaps为false

ninja|cmake/qmake|编译调试

如果没有ninja，在使用时会报错： CMake was unable to find a build program corresponding to "Ninja". CMAKE_MAKE_PROGRAM is not set. You probably need to select a different build tool.

安装

ninja需要依赖于re2c，否则编译是会报错，re2c是一款语法分析器，官网地址是：http://re2c.org/
下载页面的安装说明都在： http://re2c.org/install/install.html， 这里直接从源码安装最新版本

安装re2c：

1
2
3
4
5
git clone https://github.com/skvadrik/re2c
cd re2c
./autogen.sh 
./configure make
make install

可以从ninja的github仓库切换到release分支或者使用git下载：

1
2
git clone https://github.com/ninja-build/ninja.git
cd ninja

采用python的安装方式
1
./configure.py --bootstrap

等待完成即可

ninja比较精简，只需要一个可执行文件即可，现在可以做软链或者复制到/usr/bin下面就可以直接调用了

1
cp ninja /usr/bin/

cartographer_offline_node|激光SLAMCartographer源码解读

cartographer_offline_node对应offline_node_main.cc，它和cartographer_grpc_offline_node对应的offline_node_grpc_main.cc极为相似

在线和离线的建图效果无区别。离线节点直接从bag读取传感器数据，使用了CPU能实现的最大速度处理bag，而且不会丢失消息，不过无法设置速度和看不清建图过程的细节。 如果想控制处理速度，就使用在线节点和rosbag play -r 2，但是如果设置速度太快，可能会丢失消息，这是很常见的。

简单看了源码，离线节点之所以运行快，是因为使用了ROS多线程ros::AsyncSpinner，而在线节点还是普通的ROS程序

运行示例

1
2
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag

此时的CPU占用果然要爆了，所以说只适合运行数据集，不适合需要优化cartographer的场合
CPU占用.png

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "ros/ros.h"

int main(int argc, char** argv)
{
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  ::ros::init(argc, argv, "cartographer_offline_node");
  ::ros::start();
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  const cartographer_ros::MapBuilderFactory   map_builder_factory =
      [](const ::cartographer::mapping::proto::MapBuilderOptions& 
             map_builder_options) {
        return absl::make_unique< ::cartographer::mapping::MapBuilder>(
            const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_options));
      };
  cartographer_ros::RunOfflineNode(map_builder_factory);
  ::ros::shutdown();
}

参数

RunOfflineNode函数转到了offline_node.cc，这个文件主要就是这个函数。这个文件开头是一堆宏定义，根据字符串解释每个可用的参数

  • configuration_directory
  • configuration_basenames

  • load_state_filename

  • bag_filenames: Comma-separated list of bags to process. One bag per trajectory. Any combination of simultaneous and sequential bags is supported.

  • urdf_filenames: Comma-separated list of one or more URDF files that contain. static links for the sensor configurations

  • use_bag_transforms: 默认true，Whether to read, use and republish transforms from bags

  • keep_running: 默认false，最好改为true。Keep running the offline node after all messages from the bag have been processed

  • load_frozen_state: 默认true，Load the saved state as frozen (non-optimized) trajectories

  • skip_seconds: 默认0，Optional amount of seconds to skip from the beginning (i.e. when the earliest bag starts)

一个配置了两个机器人的的launch的关键部分是这样的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<node name="cartographer_offline_node" pkg="cartographer_ros"
  required="$(arg no_rviz)"
  type="cartographer_offline_node" args="
      -configuration_directory $(arg abspath)
      -configuration_basenames alpha-cartographer.lua,delta-cartographer.lua
      -urdf_filenames $(arg abspath)/pioneer-alpha.urdf,$(arg abspath)/pioneer-delta.urdf
      -bag_filenames $(arg abspath)/alpha.bag,$(arg abspath)/delta.bag
      -keep_running=true -use_bag_transforms=false"
  output="screen">
  <remap from="bag_1_scan" to="alpha/scan" />
  <remap from="bag_1_odom" to="alpha/pose" />
  <remap from="bag_2_scan" to="delta/scan" />
  <remap from="bag_2_odom" to="delta/pose" />
</node>

话题和service

Publications:

  • cartographer_offline_node/bagfile_progress [cartographer_ros_msgs/BagfileProgress]
  • clock [rosgraph_msgs/Clock]
  • constraint_list [visualization_msgs/MarkerArray]
  • landmark_poses_list [visualization_msgs/MarkerArray]
  • rosout [rosgraph_msgs/Log]
  • scan_matched_points2 [sensor_msgs/PointCloud2]
  • submap_list [cartographer_ros_msgs/SubmapList]
  • tf [tf2_msgs/TFMessage]
  • tf_static [tf2_msgs/TFMessage]
  • tracked_pose [geometry_msgs/PoseStamped]
  • trajectory_node_list [visualization_msgs/MarkerArray]

Subscriptions:

  • clock [rosgraph_msgs/Clock]

Services:

  • finish_trajectory
  • get_trajectory_states
  • read_metrics
  • start_trajectory
  • submap_query
  • trajectory_query
  • write_state

离线节点是直接在程序里读取消息，所以它没有像rosbag play那样把bag里的话题发布出来，我运行bag时，因为还需要运行一个节点对bag里的话题进行类型转换，而话题又没发布出来，所以离线节点实际无法使用了。

源码的重要部分

1
2
3
4
5
6
7
8
9
constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// We publish tf messages one second earlier than other messages. Under
// the assumption of higher frequency tf this should ensure that tf can
// always interpolate.
const ::ros::Duration kDelay = ::ros::Duration(1.0);
结束轨迹|激光SLAMCartographer源码解读

node_main.cc中的Run函数最后：

1
2
3
4
::ros::spin();
// 终止所有轨迹
node.FinishAllTrajectories();
node.RunFinalOptimization();

主要看FinishAllTrajectories

1
2
3
4
5
6
7
8
9
10
11
12
void Node::FinishAllTrajectories()
{
  carto::common::MutexLocker lock(&mutex_);
  for (auto& entry : is_active_trajectory_) {
    const int trajectory_id = entry.first;
    // 已经lock了，所以叫做 FinishTrajectoryUnderLock
    if (entry.second) {
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               cartographer_ros_msgs::StatusCode::OK);
    }
  }
}

offline_node.cc 中还调用了一个Node::FinishTrajectory(是这个函数唯一被调用的地方)，还有就是finish_trajectory服务的回调函数，以上3个函数实际都调用FinishTrajectoryUnderLock


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id)
{
  cartographer_ros_msgs::StatusResponse status_response;

  // First, check if we can actually finish the trajectory
  // trajectory_id 是否属于正在结束的轨迹集合，如果是 FROZEN 则返回
  if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id))
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否是 ACTIVE状态
  if (is_active_trajectory_.count(trajectory_id) == 0)
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否已经 finish
  if (!is_active_trajectory_[trajectory_id])
  {
    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
                              " has already been finished.";
    LOG(ERROR) << error;
    status_response.code =
        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
    status_response.message = error;
    return status_response;
  }
  // Shutdown the subscribers of this trajectory.
  for (auto& entry : subscribers_[trajectory_id])
  {
    entry.subscriber.shutdown();
    subscribed_topics_.erase(entry.topic);
    LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
  }
  CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  CHECK(is_active_trajectory_.at(trajectory_id));
  // 主要是这里
  map_builder_bridge_.FinishTrajectory(trajectory_id);

  is_active_trajectory_[trajectory_id] = false;
  const std::string message =
      "Finished trajectory " + std::to_string(trajectory_id) + ".";
  status_response.code = cartographer_ros_msgs::StatusCode::OK;
  status_response.message = message;
  return status_response;
}

StatusResponse不正常的code是INVALID_ARGUMENT, NOT_FOUND, RESOURCE_EXHAUSTED, 只有OK是正常的

其中的关键函数就是：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
void MapBuilderBridge::FinishTrajectory(const int trajectory_id)
{
  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
  // Make sure there is a trajectory with 'trajectory_id'.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

// 再看这个函数
void MapBuilder::FinishTrajectory(const int trajectory_id) {
  sensor_collator_->FinishTrajectory(trajectory_id);
  pose_graph_->FinishTrajectory(trajectory_id);
}

sensor_collator_就是Collator::FinishTrajectory 或者 TrajectoryCollator::FinishTrajectory，不用关注。

重要的是后端的同名函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
void PoseGraph2D::FinishTrajectory(const int trajectory_id)
{
  AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_)
  {
    absl::MutexLock  locker(&mutex_);
    CHECK(!IsTrajectoryFinished(trajectory_id));   // 不能是已结束
    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

    for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
    }
    return WorkItem::Result::kRunOptimization;
  });
}

这里又是全局优化的标志kRunOptimization，然后又去PoseGraph2D::HandleWorkQueue，也是一次优化过程了

map_builder_server.cc|激光SLAMCartographer源码解读

map_builder_server.cc中注册了一大堆回调函数，例如：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();

// get_local_to_global_transform_handler.cc 文件
void GetLocalToGlobalTransformHandler::OnRequest(
    const proto::GetLocalToGlobalTransformRequest& request)
{
  auto response = absl::make_unique<proto::GetLocalToGlobalTransformResponse>();
  auto local_to_global =
      GetContext<MapBuilderContextInterface>()
          ->map_builder()
          .pose_graph()
          ->GetLocalToGlobalTransform(request.trajectory_id());
  *response->mutable_local_to_global() = transform::ToProto(local_to_global);
  Send(std::move(response));
}

GetLocalToGlobalTransform可用于获取local坐标系的原点

1
2
3
4
5
6
7
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
    const int trajectory_id) const
{
  absl::MutexLock locker(&mutex_);
  return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
                                       trajectory_id);
}

同样的还有 PoseGraph2D::GetAllSubmapPoses, PoseGraph2D::GetTrajectoryStates, PoseGraph2D::GetTrajectoryNodePoses, PoseGraph2D::GetLandmarkPoses(), PoseGraph2D::constraints()等等

cartographer\cartographer\cloud\internal\handlers目录有一堆文件，每个文件只定义了一个对应的回调函数，这个回调函数只在map_builder_server.cc中调用，最终是运行cartographer_grpc_server

metrics机制|激光SLAMCartographer源码解读

若要启用metrics机制，必须在launch文件里添加参数-collect_metrics true，这个本质也是gflag

源码的顺序如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// node.cc中
if (collect_metrics)
{
  metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
  carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}


// register.cc 中定义
void RegisterAllMetrics(FamilyFactory* registry) {
  mapping::constraints::ConstraintBuilder2D::RegisterMetrics(registry);
  mapping::GlobalTrajectoryBuilderRegisterMetrics(registry);
  mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry);
  mapping::PoseGraph2D::RegisterMetrics(registry);
  sensor::TrajectoryCollator::RegisterMetrics(registry);
  // 不需要可以注释掉
  mapping::constraints::ConstraintBuilder3D::RegisterMetrics(registry);
  mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);
  mapping::PoseGraph3D::RegisterMetrics(registry);
}

以上所有RegisterMetrics在local_trajectory_builder_2d.cc, pose_graph_2d.cc, constraint_builder_2d.cc(排除3D情况)定义了所有metrics参数


以kLocalSlamCpuRealTimeRatio为例进行说明， 在local_trajectory_builder_2d.cc开始部分的声明：

1
static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();`

然后在LocalTrajectoryBuilder2D::RegisterMetrics中

1
2
3
4
5
auto* cpu_real_time_ratio = family_factory->NewGaugeFamily(
      "mapping_2d_local_trajectory_builder_cpu_real_time_ratio",
      "sensor duration / cpu duration.");

kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({});

最后是赋值部分

1
2
3
4
5
6
7
8
if (last_thread_cpu_time_seconds_.has_value()) {
    const double thread_cpu_duration_seconds =
        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value()) {
      kLocalSlamCpuRealTimeRatio->Set(
          common::ToSeconds(sensor_duration.value()) / thread_cpu_duration_seconds );
    }
  }

如果想查看metrics，只能rosservice call /read_metrics "{}"，一次调用的结果部分：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
name: "mapping_constraints_constraint_builder_2d_constraints"
description: "Constraints computed"
metrics: 
    type: 0
    labels: 
      - 
        key: "matcher"
        value: "searched"
      - 
        key: "search_region"
        value: "global"
    value: 4206.0
    counts_by_bucket: []
  - 
    type: 0
    labels: 
      - 
        key: "matcher"
        value: "found"
      - 
        key: "search_region"
        value: "global"
    value: 0.0
    counts_by_bucket: []

- 
  name: "mapping_2d_pose_graph_constraints"
  description: "Current number of constraints in the pose graph"
  metrics: 
    - 
      type: 1
      labels: 
        - 
          key: "tag"
          value: "inter_submap"
        - 
          key: "trajectory"
          value: "different"
      value: 0.0
      counts_by_bucket: []

name: "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers"
description: "Current number of constructed submap scan matchers"
metrics: 
- 
    type: 1
    labels: []
    value: 18.0
    counts_by_bucket: []
- 
name: "mapping_2d_pose_graph_constraints"
description: "Current number of constraints in the pose graph"
metrics: 
  - 
    type: 1
    labels: 
      - 
        key: "tag"
        value: "inter_submap"
      - 
        key: "trajectory"
        value: "different"
    value: 27.0
    counts_by_bucket: []
  - 
    type: 1
    labels: 
      - 
        key: "tag"
        value: "inter_submap"
      - 
        key: "trajectory"
        value: "same"
    value: 10.0
    counts_by_bucket: []
- 
name: "mapping_constraints_constraint_builder_2d_scores"
description: "Constraint scores built"
metrics: 
  - 
    type: 2
    labels: 
      - 
        key: "search_region"
        value: "local"

获知不同轨迹之间的查找次数和找到的约束数目，源码在ConstraintBuilder2D::ComputeConstraint中的
1
2
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);

模仿源码，可以根据自己的需要添加 metrics

如果不需要3D的metrics，可以到ConstraintBuilder3D::RegisterMetrics, LocalTrajectoryBuilder3D::RegisterMetrics, PoseGraph3D::RegisterMetrics中return

Ceres(二) 自动求导|SLAM工具

头文件只有一个"ceres/ceres.h"

调试几个经典例子的过程

1
2
3
4
5
6
7
#include "ceres/ceres.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;


1
2
3
4
5
6
7
8
9
10
11
struct  Functor
{
	template <typename T> bool operator()(const T* const x, T* residual) const
	{
		residual[0] = 10.0 - x[0];
		residual[1] = 3.0 - x[1];
		// 直接让第2维度为0，优化后的x[1]还是初值2
		// residual[1] = T(0.0);
		return true;
	}
};

这里的变量为二维的，目标函数为10.0-x和3.0-x

如果这里不写10.0而是10，就会报错。 Ceres中没有int，只接受double

对于residual[1] = T(0.0);的情况，如果不加T，编译就会报错 error: no match for ‘operator=’ (operand types are ‘ceres::Jet’ and ‘double’) . Jet类型是ceres内置类型，我们要把double转成Jet类型，也就是T(0.0)

1
2
3
4
5
6
7
8
struct  TestLocalParam
{
	template<typename T> bool operator()(const T* x, const T* delta, T* x_plus_delta) const{
		x_plus_delta[0] = x[0] + 0.4 * delta[0];
		x_plus_delta[1] = x[1] + 0.4 * delta[1];
		return true;
	}
};

CostFunctor的operator就是计算残差，不带平方项，平方是ceres自动添加，相当于最小二乘的函数 f

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// double x = 0.5;
double x[] = {0.5, 2.0};

Problem problem;

problem.AddParameterBlock(x, 2, 
  (new ceres::AutoDiffLocalParameterization
  	// 真正的维度是2， 参与优化的维度是1
  	<TestLocalParam, 2, 1> ())
	);
// AddParameterBlock 修改之后，无论下面的AutoDiffCostFunction参数是多少，只优化第1维度


// 残差的维度，参数的维度
// 如果此时是 2,1. 报错
CostFunction* cost_func = new AutoDiffCostFunction<Functor, 2, 2>(new Functor);
// 参数：CostFunction，可选的LossFunction，将CostFunction连接到一组参数块
problem.AddResidualBlock(cost_func, nullptr, x);

创建最小二乘问题，可以使用Problem::AddResidualBlock()和Problem::AddParameterBlock()。 可以使用Problem::AddParameterBlock()显式添加参数块，这将导致额外的正确性检查; 然而，如果参数块不存在，Problem::AddResidualBlock()会隐式添加参数块。

AddParameterBlock()显式地向Problem添加了一个参数块。它还允许用户将LocalParameterization对象与参数块关联起来。

AddResidualBlock默认会先调用AddParameterBlock，一般不用后者。

损失函数LossFunction，给异常值做限定，如果是nullptr，该项的代价就是残差的平方范数

代价函数携带关于它所期望的参数块大小的信息。函数检查这些参数是否与parameter_blocks中列出的参数块的大小匹配。如果检测到不匹配，程序将中止。

Solver

1
2
3
4
5
6
7
8
9
10
11
12
13
14
  Solver::Options  option;
  option.minimizer_progress_to_stdout = true;
  option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  Solver::Summary summary;
  Solve(option, &problem, &summary);
  
  std::cout << summary.BriefReport() << std::endl<<std::endl;
  // std::cout << summary.FullReport() << std::endl<<std::endl;

  // std::cout << "initial x: " << initial_x <<"   x: "<< x << std::endl;
  std::cout <<"x[0]: "<< x[0] <<"   x[1]: "<< x[1]<< std::endl;
  return 0;
}

优先选用自动微分算法，某些情况可能需要用到解析微分算法，尽量避免数值微分算法。

曲线拟合

使用十四讲第六章的曲线拟合的例子，已知 N 个数据 ，它们符合曲线 ，通过曲线拟合求 a, b, c

跟上面的例子不同，这次目标函数为 ， 残差维度为3(未知a,b,c)

CMake配置如下

1
2
3
4
5
6
7
8
find_package(OpenCV REQUIRED)
INCLUDE_DIRECTORIES(${OpenCV_DIRS})

target_link_libraries(node_2
   ${catkin_LIBRARIES}
   ${CERES_LIBRARIES}
   ${OpenCV_LIBS}
)

先生成带有高斯噪声的数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#include <opencv2/opencv.hpp>

// 真实值
double a =1.0, b=2.0, c=1.0;
// abc的初值
double abc[3]  = {0,0,0};

// 生成带有噪声的模拟数据
vector<double> x_data, y_data;
int N = 240;  // 数据数量
double w_sigma = 1.0;  // 高斯标准差
cv::RNG  rng;
for(unsigned int i =0; i<N; i++)
{
    double x = i/100.0;
    x_data.push_back(x);
    y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
}

高斯噪声的产生： cv::RNG rng; rng.gaussian(w_sigma)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
struct Functor
{
    Functor(double x, double y):
    m_x(x), m_y(y) {    }
    #if 0
    // 开始我是这么写的，没弄清残差的维度
    template<typename T> bool operator()( const T* const a, 
                                          const T* const b, 
                                          const T* const c, 
                                          T* residual)  const
    {
        residual[0] = m_y - exp(a[0] * m_x * m_x + b[0] * m_x + c[0]);
        return true;
    }
    #endif

    // 因为要传入数据(x,y)， 所以要定义两个成员变量
    template<typename T> bool operator()( const T* const abc, T* residual)  const
    {
        residual[0] = m_y - exp(abc[0] * m_x * m_x + abc[1] * m_x + abc[2]);
        return true;
    }
    const double m_x, m_y;
};

因为自变量x是一维的，而不是向量，所以残差只计算一维 residual[0]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
Problem problem;
for(unsigned int i=0; i<N; i++)
{
	// 残差维度， 所估计变量的维度
  CostFunction* cost_func = new AutoDiffCostFunction<Functor,1,3>(new Functor(x_data[i], y_data[i]) );
  problem.AddResidualBlock(cost_func, nullptr, abc);
}
Solver::Options option;
option.minimizer_progress_to_stdout = true;
option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

Solver::Summary summary;
Solve(option, &problem, &summary);

cout << summary.BriefReport() << endl <<endl;
cout << "a: "<< abc[0] <<"  b: "<< abc[1]<< "  c: "<< abc[2] <<endl;

参考：
Curve Fitting(曲线拟合)
Problem类
资料

cartographer_grpc_server的使用和配置|激光SLAMCartographer原理和配置

使用gRPC与云端结合：cartographer使用Protobuf数据传输协议，所以可以使用gRPC（也是谷歌的），可以通过云端运行cartographer算法，最典型的例子就是多机器人在一张已知地图上进行导航，在一个远程的强大centralized localization server运行 multi-trajectories Cartographer

方案：一台机器运行local optimization，另一台运行 global optimization

Cloud-based mapping ，using a gRPC streaming connection between local and global SLAM to propagate sensor data and local SLAM updates, which does not support ACKs and resending, hence this is not going to work over spotty WiFi.

先安装protobuf和ceres

安装 grpc

一开始下载grpc后进行编译，结果报错：

1
2
3
4
CMake Warning at cmake/abseil-cpp.cmake:30 (message):
  gRPC_ABSL_PROVIDER is "module" but ABSL_ROOT_DIR is wrong
Call Stack (most recent call first):
  CMakeLists.txt:188 (include)

这说明第三方库没准备好，最好按如下步骤：

如果只想下载指定版本的，如以版本1.27.3为例，可改成如下语句：

1
git clone -b v1.27.0 https://github.com/grpc/grpc.git

上列操作成功完成后，gRPC 源码的第三方依赖目录 third_party 实际是空的，需通过下列步骤拉取依赖的第三方。切换到 grpc 目录，下载 grpc 第三方依赖到本地：
1
git submodule update --init

grpc依赖的第三方库有点多，如果不借助git submodule，手工一个个下载不太容易。其中的bloaty库很大。

注意，gRPC 要求 CMake 3.5.1 或以上版本的CMake，否则会报错 CMake 3.5.1 or higher is required

编译：

1
2
3
4
5
6
7
8
9
10
11
cmake -DgRPC_SSL_PROVIDER=package ..
make -j8
sudo make install

cmake  -DgRPC_INSTALL=ON -DgRPC_ZLIB_PROVIDER=package -DgRPC_CARES_PROVIDER=package -DgRPC_PROTOBUF_PROVIDER=package -DgRPC_SSL_PROVIDER=package -DgRPC_ABSL_PROVIDER=package ..
make -j8
sudo make install

cmake  -DgRPC_INSTALL=ON -DBUILD_SHARED_LIBS=ON -DgRPC_ZLIB_PROVIDER=package -DgRPC_CARES_PROVIDER=package -DgRPC_PROTOBUF_PROVIDER=package -DgRPC_SSL_PROVIDER=package -DgRPC_ABSL_PROVIDER=package ..
make -j8
sudo make install

CMake配置

在包含option(BUILD_GRPC "build Cartographer gRPC support" false)的3个CMakeLists修改为true。 最好再添加 set(BUILD_GRPC true)，以及C++14的支持 set(CMAKE_CXX_STANDARD 14)

lua配置

backpack_2d_server.lua如下:

1
2
3
4
include "map_builder_server.lua"

MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
return MAP_BUILDER_SERVER

map_builder_server.lua如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
include "map_builder.lua"

MAP_BUILDER_SERVER = {
  map_builder = MAP_BUILDER,
  num_event_threads = 4,
  num_grpc_threads = 4,
  --- 我的修改
  server_address = "192.168.0.105:50051",
  uplink_server_address = "",
  upload_batch_size = 100,
  enable_ssl_encryption = false,
  enable_google_auth = false,
}

运行数据集

1
roslaunch cartographer_ros grpc_demo_backpack_2d.launch bag_filename:=/home/user/cartographer/dataset/cartographer_paper_deutsches_museum.bag

这是在一台机上运行客户端和服务端

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
<launch>
  <param name="/use_sim_time" value="true" />
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
  <!-- 服务端 -->
  <node name="cartographer_grpc_server" pkg="cartographer_ros"
      type="cartographer_grpc_server.sh" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_server.lua">
  </node>
  <!-- 客户端 -->
  <node name="cartographer_grpc_node" pkg="cartographer_ros"
      type="cartographer_grpc_node" args="
    -client_id CLIENT_ID
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

  <node name="playbag" pkg="rosbag" type="play"
        args="--clock $(arg bag_filename)" />
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

就是CLIENT_ID，无需设置为数字。

真实运行

  • 把客户端上的ROS_MASTER_URI设置为服务端的IP
  • rviz放在服务端。
  • 服务端的终端刷新日志，客户端不刷新
  • 网络不能差，否则服务端刷新慢。
  • 连接成功后，关服务端，客户端也会终止。连接成功后，关客户端，服务端停止刷新；再启动客户端，服务端继续更新。

客户端

1
2
3
4
5
6
7
8
9
10
11
12
<launch>
  <node name="cartographer_grpc_node" pkg="cartographer_ros"
      type="cartographer_grpc_node" args="
      -client_id CLIENT_ID
      -server_address 192.168.1.213:50051
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
      <remap from="/scan" to="/sick_tim551_scan" />
      <remap from="/odom" to="/odometry/filtered" />
  </node>
</launch>

服务端

1
2
3
4
5
6
7
8
9
10
<launch>
  <node name="cartographer_grpc_server" pkg="cartographer_ros"
      type="cartographer_grpc_server.sh" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_server.lua">
  </node>

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

纯定位

1
2
3
4
5
6
7
8
9
<node name="cartographer_grpc_node" pkg="cartographer_ros"
    type="cartographer_grpc_node" args="
        -client_id CLIENT_ID
        -configuration_directory $(find cartographer_ros)/configuration_files
        -configuration_basename backpack_2d_localization.lua
        -load_state_filename $(arg load_state_filename)"
    output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
</node>

参考：
运行cartographer的gRPC demo
Ubuntu下安装c-ares库

如何打印当前使用的lua参数|激光SLAMCartographer原理和配置

这里就是使用cartographer_print_configuration，它输出了当前cartographer使用的所有lua参数，虽然不如rosparam强大，但也是一种方法，本质就是读取加载的lua文件。

代码比较简单，不用太过研究，直接写launch

1
2
3
4
5
6
7
8
9
<launch>
  <node name="print_mit_config" pkg="cartographer_ros"
      type="cartographer_print_configuration" args="
          -configuration_directories $(find cartographer_ros)/configuration_files
          -configuration_basename mit_stata_odom.lua
	  	  -subdictionary map_builder"
      output="screen">
  </node>
</launch>

运行后会报错，找不到cartographer_print_configuration，这是因为可执行文件在bin路径: /home/user/carto_ws/install_isolated/bin/cartographer_print_configuration，roslaunch找的是/home/user/carto_ws/install_isolated/lib/cartographer_ros路径下的文件，复制到这里就好了。

运行launch就会输出所有lua参数，自己设置的参数也会覆盖默认的。 另外还有个subdictionary参数，输出的是子参数，这里用map_builder。但是只能输出第一级的子参数，如果自参数太底层就会报错，比如submaps做子参数就会运行错误。