cartographer 处理IMU（激光，里程计等）流程
1、cartographer_ros
入口文件：node_main.cc
入口函数main,如下图：
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();2、void Run() 启动轨迹处
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}3、文件node.cc
AddTrajectory(options, DefaultSensorTopics()); 函数
LaunchSubscribers(options, topics, trajectory_id); 订阅有关话题
4、LaunchSubscribers
{SubscribeWithHandler(&Node::HandleImuMessage,
trajectory_id, topic,
&node_handle_, this), 5、HandleImuMessage兵分两路 一路用于位姿推算 一路用于后面的计算
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);6、位姿推算这儿
文件：cartographer/mapping/nternal/pose_extrapolator.cc
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
CHECK(timed_pose_queue_.empty() ||
imu_data.time >= timed_pose_queue_.back().time);
imu_data_.push_back(imu_data);
TrimImuData();
}7、后面的IMU运算到了 sensor_bridge.cc
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr imu_data = ToImuData(msg);
if (imu_data != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity});
}
}
8、在添加轨迹中定义了变量 CollatedTrajectoryBuilder 继承TrajectoryBuilderInterface（虚函数，接口）
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddData(sensor::MakeDispatchable(sensor_id, imu_data));
}9、而 sensor_collator_->AddSensorData中， sensor::Collator sensor_collator_;在map_builder.h中定义了。继而找到在collator.cc中找到了实现
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
}10、这里就会把IMU数据加入队列啦。std::unique_ptr是标准库的智能指针，与auto_ptr真的非常类似，auto_ptr可以说可以随便赋值，但赋值完后原来的对象就不知不觉报废，而unique_ptr不让你随便赋值，只能move内存转移。
11、注意到在collator.h中一个函数： （ sensor/internal/collator.cc）
void Collator::AddTrajectory(
const int trajectory_id,
const std::unordered_set& expected_sensor_ids,
const Callback& callback) {
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{trajectory_id, sensor_id};
queue_.AddQueue(queue_key,
[callback, sensor_id](std::unique_ptr data) {
callback(sensor_id, std::move(data));
});
queue_keys_[trajectory_id].push_back(queue_key);
}
} 在 CollatedTrajectoryBuilder的构造函数中回调了 HandleCollatedSensorData
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( //整理的轨迹生成器
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set& expected_sensor_ids,
std::unique_ptr wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
collate_landmarks_(trajectory_options.collate_landmarks()),
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
std::unordered_set expected_sensor_id_strings;
for (const auto& sensor_id : expected_sensor_ids) {
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
!collate_landmarks_) {
continue;
}
if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
!collate_fixed_frame_) {
continue;
}
expected_sensor_id_strings.insert(sensor_id.id);
}
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr data) {
HandleCollatedSensorData(sensor_id, std::move(data));
});
}
12、它在CollatedTrajectoryBuilder的构造函数中调用了，传入的回调函数名为HandleCollatedSensorData，处理整理后的传感器数据，到这里就开始进入关键正题啦！
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr data) {
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) {
it = rate_timers_
.emplace(
std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
.first;
}
it->second.Pulse(data->GetTime());
if (std::chrono::steady_clock::now() - last_logging_time_ >
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
for (const auto& pair : rate_timers_) {
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_logging_time_ = std::chrono::steady_clock::now();
}
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
} 13、在头文件cartographer/mapping_2d/global_trajectory_builder.cc中，知道GlobalTrajectoryBuilder类继承了GlobalTrajectoryBuilderInterface类，如图：
template
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
public: 14、自然就能找到GlobalTrajectoryBuilder::AddImuData的具体实现了
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}15、pose_graph是PoseGraph2D的父类，虚函数，多态
void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}16、OptimizationProblem类
void OptimizationProblem2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
imu_data_.Append(trajectory_id, imu_data);
}
17、map_by_time.h 将数据附加到'trajectory_id'，根据需要创建轨迹。
void Append(const int trajectory_id, const DataType& data) {
CHECK_GE(trajectory_id, 0);
auto& trajectory = data_[trajectory_id];
if (!trajectory.empty()) {
CHECK_GT(data.time, std::prev(trajectory.end())->first);
}
trajectory.emplace(data.time, data);
}