Showing preview only (1,062K chars total). Download the full file or copy to clipboard to get everything.
Repository: slam-code/cartographer
Branch: master
Commit: 4332493a434d
Files: 273
Total size: 954.9 KB
Directory structure:
gitextract_6vczu7ti/
├── .gitignore
├── README.md
├── common/
│ ├── blocking_queue.h
│ ├── blocking_queue_test.cc
│ ├── ceres_solver_options.cc
│ ├── ceres_solver_options.h
│ ├── config.h
│ ├── config.h.cmake
│ ├── configuration_file_resolver.cc
│ ├── configuration_file_resolver.h
│ ├── fixed_ratio_sampler.cc
│ ├── fixed_ratio_sampler.h
│ ├── fixed_ratio_sampler_test.cc
│ ├── histogram.cc
│ ├── histogram.h
│ ├── lua.h
│ ├── lua_parameter_dictionary.cc
│ ├── lua_parameter_dictionary.h
│ ├── lua_parameter_dictionary_test.cc
│ ├── lua_parameter_dictionary_test_helpers.h
│ ├── make_unique.h
│ ├── make_unique_test.cc
│ ├── math.h
│ ├── math_test.cc
│ ├── mutex.h
│ ├── port.h
│ ├── port_le_test.cpp
│ ├── proto/
│ │ └── ceres_solver_options.proto
│ ├── rate_timer.h
│ ├── rate_timer_test.cc
│ ├── thread_pool.cc
│ ├── thread_pool.h
│ ├── time.cc
│ └── time.h
├── ground_truth/
│ └── compute_relations_metrics_main.cc
├── io/
│ ├── cairo_types.h
│ ├── coloring_points_processor.cc
│ ├── coloring_points_processor.h
│ ├── counting_points_processor.cc
│ ├── counting_points_processor.h
│ ├── file_writer.cc
│ ├── file_writer.h
│ ├── fixed_ratio_sampling_points_processor.cc
│ ├── fixed_ratio_sampling_points_processor.h
│ ├── intensity_to_color_points_processor.cc
│ ├── intensity_to_color_points_processor.h
│ ├── min_max_range_filtering_points_processor.cc
│ ├── min_max_range_filtering_points_processor.h
│ ├── null_points_processor.h
│ ├── outlier_removing_points_processor.cc
│ ├── outlier_removing_points_processor.h
│ ├── pcd_writing_points_processor.cc
│ ├── pcd_writing_points_processor.h
│ ├── ply_writing_points_processor.cc
│ ├── ply_writing_points_processor.h
│ ├── points_batch.cc
│ ├── points_batch.h
│ ├── points_processor.h
│ ├── points_processor_pipeline_builder.cc
│ ├── points_processor_pipeline_builder.h
│ ├── proto_stream.cc
│ ├── proto_stream.h
│ ├── proto_stream_test.cc
│ ├── xray_points_processor.cc
│ ├── xray_points_processor.h
│ ├── xyz_writing_points_processor.cc
│ └── xyz_writing_points_processor.h
├── kalman_filter/
│ ├── gaussian_distribution.h
│ ├── gaussian_distribution_test.cc
│ ├── pose_tracker.cc
│ ├── pose_tracker.h
│ ├── pose_tracker_test.cc
│ ├── proto/
│ │ └── pose_tracker_options.proto
│ ├── unscented_kalman_filter.h
│ └── unscented_kalman_filter_test.cc
├── mapping/
│ ├── collated_trajectory_builder.cc
│ ├── collated_trajectory_builder.h
│ ├── detect_floors.cc
│ ├── detect_floors.h
│ ├── global_trajectory_builder_interface.h
│ ├── id.h
│ ├── imu_tracker.cc
│ ├── imu_tracker.h
│ ├── map_builder.cc
│ ├── map_builder.h
│ ├── odometry_state_tracker.cc
│ ├── odometry_state_tracker.h
│ ├── probability_values.cc
│ ├── probability_values.h
│ ├── probability_values_test.cc
│ ├── proto/
│ │ ├── map_builder_options.proto
│ │ ├── sparse_pose_graph.proto
│ │ ├── sparse_pose_graph_options.proto
│ │ ├── submap_visualization.proto
│ │ ├── trajectory.proto
│ │ ├── trajectory_builder_options.proto
│ │ └── trajectory_connectivity.proto
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem_options.cc
│ │ ├── optimization_problem_options.h
│ │ └── proto/
│ │ ├── constraint_builder_options.proto
│ │ └── optimization_problem_options.proto
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── submaps.cc
│ ├── submaps.h
│ ├── submaps_test.cc
│ ├── trajectory_builder.cc
│ ├── trajectory_builder.h
│ ├── trajectory_connectivity.cc
│ ├── trajectory_connectivity.h
│ ├── trajectory_connectivity_test.cc
│ └── trajectory_node.h
├── mapping_2d/
│ ├── global_trajectory_builder.cc
│ ├── global_trajectory_builder.h
│ ├── local_trajectory_builder.cc
│ ├── local_trajectory_builder.h
│ ├── local_trajectory_builder_options.cc
│ ├── local_trajectory_builder_options.h
│ ├── map_limits.h
│ ├── map_limits_test.cc
│ ├── probability_grid.h
│ ├── probability_grid_test.cc
│ ├── proto/
│ │ ├── cell_limits.proto
│ │ ├── local_trajectory_builder_options.proto
│ │ ├── map_limits.proto
│ │ ├── probability_grid.proto
│ │ ├── range_data_inserter_options.proto
│ │ └── submaps_options.proto
│ ├── range_data_inserter.cc
│ ├── range_data_inserter.h
│ ├── range_data_inserter_test.cc
│ ├── ray_casting.cc
│ ├── ray_casting.h
│ ├── scan_matching/
│ │ ├── ceres_scan_matcher.cc
│ │ ├── ceres_scan_matcher.h
│ │ ├── ceres_scan_matcher_test.cc
│ │ ├── correlative_scan_matcher.cc
│ │ ├── correlative_scan_matcher.h
│ │ ├── correlative_scan_matcher_test.cc
│ │ ├── fast_correlative_scan_matcher.cc
│ │ ├── fast_correlative_scan_matcher.h
│ │ ├── fast_correlative_scan_matcher_test.cc
│ │ ├── fast_global_localizer.cc
│ │ ├── fast_global_localizer.h
│ │ ├── occupied_space_cost_functor.h
│ │ ├── proto/
│ │ │ ├── ceres_scan_matcher_options.proto
│ │ │ ├── fast_correlative_scan_matcher_options.proto
│ │ │ └── real_time_correlative_scan_matcher_options.proto
│ │ ├── real_time_correlative_scan_matcher.cc
│ │ ├── real_time_correlative_scan_matcher.h
│ │ ├── real_time_correlative_scan_matcher_test.cc
│ │ ├── rotation_delta_cost_functor.h
│ │ └── translation_delta_cost_functor.h
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem.cc
│ │ ├── optimization_problem.h
│ │ └── spa_cost_function.h
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── sparse_pose_graph_test.cc
│ ├── submaps.cc
│ ├── submaps.h
│ ├── submaps_test.cc
│ ├── xy_index.h
│ └── xy_index_test.cc
├── mapping_3d/
│ ├── acceleration_cost_function.h
│ ├── ceres_pose.cc
│ ├── ceres_pose.h
│ ├── global_trajectory_builder.cc
│ ├── global_trajectory_builder.h
│ ├── hybrid_grid.h
│ ├── hybrid_grid_test.cc
│ ├── imu_integration.cc
│ ├── imu_integration.h
│ ├── kalman_local_trajectory_builder.cc
│ ├── kalman_local_trajectory_builder.h
│ ├── kalman_local_trajectory_builder_options.cc
│ ├── kalman_local_trajectory_builder_options.h
│ ├── kalman_local_trajectory_builder_test.cc
│ ├── local_trajectory_builder.cc
│ ├── local_trajectory_builder.h
│ ├── local_trajectory_builder_interface.h
│ ├── local_trajectory_builder_options.cc
│ ├── local_trajectory_builder_options.h
│ ├── motion_filter.cc
│ ├── motion_filter.h
│ ├── motion_filter_test.cc
│ ├── optimizing_local_trajectory_builder.cc
│ ├── optimizing_local_trajectory_builder.h
│ ├── optimizing_local_trajectory_builder_options.cc
│ ├── optimizing_local_trajectory_builder_options.h
│ ├── proto/
│ │ ├── hybrid_grid.proto
│ │ ├── kalman_local_trajectory_builder_options.proto
│ │ ├── local_trajectory_builder_options.proto
│ │ ├── motion_filter_options.proto
│ │ ├── optimizing_local_trajectory_builder_options.proto
│ │ ├── range_data_inserter_options.proto
│ │ └── submaps_options.proto
│ ├── range_data_inserter.cc
│ ├── range_data_inserter.h
│ ├── range_data_inserter_test.cc
│ ├── rotation_cost_function.h
│ ├── scan_matching/
│ │ ├── ceres_scan_matcher.cc
│ │ ├── ceres_scan_matcher.h
│ │ ├── ceres_scan_matcher_test.cc
│ │ ├── fast_correlative_scan_matcher.cc
│ │ ├── fast_correlative_scan_matcher.h
│ │ ├── fast_correlative_scan_matcher_test.cc
│ │ ├── interpolated_grid.h
│ │ ├── interpolated_grid_test.cc
│ │ ├── occupied_space_cost_functor.h
│ │ ├── precomputation_grid.cc
│ │ ├── precomputation_grid.h
│ │ ├── precomputation_grid_test.cc
│ │ ├── proto/
│ │ │ ├── ceres_scan_matcher_options.proto
│ │ │ └── fast_correlative_scan_matcher_options.proto
│ │ ├── real_time_correlative_scan_matcher.cc
│ │ ├── real_time_correlative_scan_matcher.h
│ │ ├── real_time_correlative_scan_matcher_test.cc
│ │ ├── rotation_delta_cost_functor.h
│ │ ├── rotational_scan_matcher.cc
│ │ ├── rotational_scan_matcher.h
│ │ └── translation_delta_cost_functor.h
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem.cc
│ │ ├── optimization_problem.h
│ │ ├── optimization_problem_test.cc
│ │ └── spa_cost_function.h
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── submaps.cc
│ ├── submaps.h
│ └── translation_cost_function.h
├── sensor/
│ ├── collator.cc
│ ├── collator.h
│ ├── collator_test.cc
│ ├── compressed_point_cloud.cc
│ ├── compressed_point_cloud.h
│ ├── compressed_point_cloud_test.cc
│ ├── configuration.cc
│ ├── configuration.h
│ ├── data.h
│ ├── ordered_multi_queue.cc
│ ├── ordered_multi_queue.h
│ ├── ordered_multi_queue_le_test.cc
│ ├── ordered_multi_queue_test.cc
│ ├── point_cloud.cc
│ ├── point_cloud.h
│ ├── point_cloud_test.cc
│ ├── proto/
│ │ ├── adaptive_voxel_filter_options.proto
│ │ ├── configuration.proto
│ │ └── sensor.proto
│ ├── range_data.cc
│ ├── range_data.h
│ ├── range_data_test.cc
│ ├── voxel_filter.cc
│ ├── voxel_filter.h
│ └── voxel_filter_test.cc
└── transform/
├── proto/
│ └── transform.proto
├── rigid_transform.cc
├── rigid_transform.h
├── rigid_transform_le2_grid2_test.cc
├── rigid_transform_test_helpers.h
├── transform.cc
├── transform.h
├── transform_interpolation_buffer.cc
├── transform_interpolation_buffer.h
├── transform_interpolation_buffer_test.cc
└── transform_test.cc
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# 忽略*.o和*.a文件
*.[oa]
*.[out]
*build
build #过滤整个build文件夹
devel
*.class #过滤所有.class文件
.idea
cartographer
cartographer_ros
ceres-solver
Eigen
src
.zip
orig
backup
pictures
html
note
================================================
FILE: README.md
================================================
# cartographer
关于谷歌slam地图库 [cartographer](https://github.com/googlecartographer/cartographer)的源码注释。(只含源码)
博客文章：
* [知乎](https://zhuanlan.zhihu.com/learnmoreonce) (简略)
* [CSDN](http://blog.csdn.net/learnmoreonce) (详细)
源码分析的markdown文件可在 [这里]( https://github.com/slam4code/SLAM/tree/master/cartographer-%E6%BA%90%E7%A0%81%E5%88%86%E6%9E%90 )
查找。
我的微信公众号:slamcode
<div align="center">
<img src="https://raw.githubusercontent.com/slam4code/SLAM/master/slamcode.jpg" width = "200" height = "200" alt="slamcode" align=center />
</div>
================================================
FILE: common/blocking_queue.h
================================================
#ifndef CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
#define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
#include <cstddef>
#include <deque>
#include <memory>
#include "cartographer/common/mutex.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "glog/logging.h"
/*
* BlockingQueue是线程安全的阻塞队列,(生产者消费者模式)
* 不可拷贝不可赋值
* 构造函数初始化队列大小,kInfiniteQueueSize=0默认不限制容量。queue_size限制容量：通过条件变量做到.
* Push()添加元素,容量不够时,阻塞等待
* Pop()删除元素,没有元素时,阻塞等待
* Peek()返回下一个应该弹出的元素
* PushWithTimeout(),添加元素,若超时则返回false
* PopWithTimeout(),删除元素，若超时则返回false
*
* */
namespace cartographer {
namespace common {
// A thread-safe blocking queue that is useful for producer/consumer patterns.
// 'T' must be movable.
template <typename T>
class BlockingQueue {
public:
static constexpr size_t kInfiniteQueueSize = 0;
// Constructs a blocking queue with infinite queue size.
BlockingQueue() : BlockingQueue(kInfiniteQueueSize) {} //默认不限制容量
BlockingQueue(const BlockingQueue&) = delete;
BlockingQueue& operator=(const BlockingQueue&) = delete;
// Constructs a blocking queue with a size of 'queue_size'.限制容量
explicit BlockingQueue(const size_t queue_size) : queue_size_(queue_size) {}
// Pushes a value onto the queue. Blocks if the queue is full.
void Push(T t) {
MutexLocker lock(&mutex_);//加锁
lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); });//队列未满才能push
deque_.push_back(std::move(t)); //移动而非拷贝
}
// Like push, but returns false if 'timeout' is reached.
bool PushWithTimeout(T t, const common::Duration timeout) { //最多等待timeout时间
MutexLocker lock(&mutex_);
if (!lock.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { return QueueNotFullCondition(); },
timeout)) {
return false;
}
deque_.push_back(std::move(t));
return true;
}
// Pops the next value from the queue. Blocks until a value is available.
T Pop() {
MutexLocker lock(&mutex_); //加锁
lock.Await([this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); });//队列不为空才能pop()
T t = std::move(deque_.front());
deque_.pop_front();
return t;
}
// Like Pop, but can timeout. Returns nullptr in this case.
T PopWithTimeout(const common::Duration timeout) {
MutexLocker lock(&mutex_);
if (!lock.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); },
timeout)) {
return nullptr;
}
T t = std::move(deque_.front());
deque_.pop_front();
return t;
}
// Returns the next value in the queue or nullptr if the queue is empty.
// Maintains ownership. This assumes a member function get() that returns
// a pointer to the given type R.
template <typename R>
const R* Peek() {
MutexLocker lock(&mutex_);
if (deque_.empty()) {
return nullptr;
}
return deque_.front().get();
}
// Returns the number of items currently in the queue.
size_t Size() {
MutexLocker lock(&mutex_);
return deque_.size();
}
private:
// Returns true iff the queue is not empty.
bool QueueNotEmptyCondition() REQUIRES(mutex_) { return !deque_.empty(); }
// Returns true iff the queue is not full.
bool QueueNotFullCondition() REQUIRES(mutex_) {
return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
}
Mutex mutex_;
const size_t queue_size_ GUARDED_BY(mutex_);
std::deque<T> deque_ GUARDED_BY(mutex_);
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
================================================
FILE: common/blocking_queue_test.cc
================================================
#include "cartographer/common/blocking_queue.h"
#include <memory>
#include <thread>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace common {
namespace {
TEST(BlockingQueueTest, testPushPeekPop) {
BlockingQueue<std::unique_ptr<int>> blocking_queue;//元素是智能指针,不限容
blocking_queue.Push(common::make_unique<int>(42)); //添加
ASSERT_EQ(1, blocking_queue.Size()); //size==1
blocking_queue.Push(common::make_unique<int>(24)); //添加
ASSERT_EQ(2, blocking_queue.Size()); //size==2
EXPECT_EQ(42, *blocking_queue.Peek<int>()); //顶元素==42
ASSERT_EQ(2, blocking_queue.Size()); //队列大小是2
EXPECT_EQ(42, *blocking_queue.Pop());
ASSERT_EQ(1, blocking_queue.Size());
EXPECT_EQ(24, *blocking_queue.Pop());
ASSERT_EQ(0, blocking_queue.Size());
EXPECT_EQ(nullptr, blocking_queue.Peek<int>());
ASSERT_EQ(0, blocking_queue.Size());
}
TEST(BlockingQueueTest, testPushPopSharedPtr) {
BlockingQueue<std::shared_ptr<int>> blocking_queue; //不限容
blocking_queue.Push(std::make_shared<int>(42));
blocking_queue.Push(std::make_shared<int>(24));
EXPECT_EQ(42, *blocking_queue.Pop());
EXPECT_EQ(24, *blocking_queue.Pop());
}
TEST(BlockingQueueTest, testPopWithTimeout) {
BlockingQueue<std::unique_ptr<int>> blocking_queue;
EXPECT_EQ(nullptr, //没有插入,所以是nullptr
blocking_queue.PopWithTimeout(common::FromMilliseconds(150)));
}
TEST(BlockingQueueTest, testPushWithTimeout) {
BlockingQueue<std::unique_ptr<int>> blocking_queue(1);//限容
EXPECT_EQ(true, //插入成功
blocking_queue.PushWithTimeout(common::make_unique<int>(42),
common::FromMilliseconds(150)));
EXPECT_EQ(false, //失败,因为队列大小是1
blocking_queue.PushWithTimeout(common::make_unique<int>(15),
common::FromMilliseconds(150)));
EXPECT_EQ(42, *blocking_queue.Pop());
EXPECT_EQ(0, blocking_queue.Size());
}
TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) {
BlockingQueue<std::unique_ptr<int>> blocking_queue; //不限容量
EXPECT_EQ(true,
blocking_queue.PushWithTimeout(common::make_unique<int>(42),
common::FromMilliseconds(150)));
EXPECT_EQ(true,
blocking_queue.PushWithTimeout(common::make_unique<int>(45),
common::FromMilliseconds(150)));
EXPECT_EQ(42, *blocking_queue.Pop());
EXPECT_EQ(45, *blocking_queue.Pop());
EXPECT_EQ(0, blocking_queue.Size());
}
TEST(BlockingQueueTest, testBlockingPop) {
BlockingQueue<std::unique_ptr<int>> blocking_queue;
ASSERT_EQ(0, blocking_queue.Size());
int pop = 0;
//多线程测试
std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.Pop(); });
std::this_thread::sleep_for(common::FromMilliseconds(100));
blocking_queue.Push(common::make_unique<int>(42));
thread.join();
ASSERT_EQ(0, blocking_queue.Size());
EXPECT_EQ(42, pop);
}
TEST(BlockingQueueTest, testBlockingPopWithTimeout) {
BlockingQueue<std::unique_ptr<int>> blocking_queue;
ASSERT_EQ(0, blocking_queue.Size());
int pop = 0;
//多线程测试
std::thread thread([&blocking_queue, &pop] {
pop = *blocking_queue.PopWithTimeout(common::FromMilliseconds(2500));
});
std::this_thread::sleep_for(common::FromMilliseconds(100));
blocking_queue.Push(common::make_unique<int>(42));
thread.join();
ASSERT_EQ(0, blocking_queue.Size());
EXPECT_EQ(42, pop);
}
} // namespace
} // namespace common
} // namespace cartographer
================================================
FILE: common/ceres_solver_options.cc
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/common/ceres_solver_options.h"
namespace cartographer {
namespace common {
/*
根据lua文件设置
*/
proto::CeresSolverOptions CreateCeresSolverOptionsProto(
common::LuaParameterDictionary* parameter_dictionary)
{
proto::CeresSolverOptions proto;
proto.set_use_nonmonotonic_steps(
parameter_dictionary->GetBool("use_nonmonotonic_steps"));//是否设置使用nonmonotonic
proto.set_max_num_iterations(
parameter_dictionary->GetNonNegativeInt("max_num_iterations"));//迭代次数
proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads"));//线程数
CHECK_GT(proto.max_num_iterations(), 0);
CHECK_GT(proto.num_threads(), 0);
return proto;
}
/*
根据proto文件设置
*/
ceres::Solver::Options CreateCeresSolverOptions(
const proto::CeresSolverOptions& proto)
{
ceres::Solver::Options options;//建立临时对象
options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps();//为临时对象赋值
options.max_num_iterations = proto.max_num_iterations(); //赋值
options.num_threads = proto.num_threads(); //赋值
return options;
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/ceres_solver_options.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_
#define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/proto/ceres_solver_options.pb.h"
#include "ceres/ceres.h"
namespace cartographer {
namespace common {
/*
配置加载类, 根据lua设置 CeresSolver的选项
*/
proto::CeresSolverOptions CreateCeresSolverOptionsProto(
common::LuaParameterDictionary* parameter_dictionary);
//根据proto设置 CeresSolver的选项
ceres::Solver::Options CreateCeresSolverOptions(
const proto::CeresSolverOptions& proto);
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_
================================================
FILE: common/config.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_CONFIG_H_
#define CARTOGRAPHER_COMMON_CONFIG_H_
namespace cartographer {
namespace common {
constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径
"";
constexpr char kSourceDirectory[] = "./";
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_CONFIG_H_
================================================
FILE: common/config.h.cmake
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_CONFIG_H_
#define CARTOGRAPHER_COMMON_CONFIG_H_
namespace cartographer {
namespace common {
constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径
"@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@";
constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@";
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_CONFIG_H_
================================================
FILE: common/configuration_file_resolver.cc
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/common/configuration_file_resolver.h"
#include <fstream>
#include <iostream>
#include <streambuf>
#include "cartographer/common/config.h"//根据config.h.cmake生成
#include "glog/logging.h"
namespace cartographer {
namespace common {
ConfigurationFileResolver::ConfigurationFileResolver(
const std::vector<string>& configuration_files_directories)
: configuration_files_directories_(configuration_files_directories) {
configuration_files_directories_.push_back(kConfigurationFilesDirectory);//配置文件夹路径
}
string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) { //根据文件 夹和basename生成文件名
for (const auto& path : configuration_files_directories_) {
const string filename = path + "/" + basename;
std::ifstream stream(filename.c_str());
if (stream.good()) {
LOG(INFO) << "Found '" << filename << "' for '" << basename << "'.";
return filename;
}
}
LOG(FATAL) << "File '" << basename << "' was not found.";
}
string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) {//根据文件名读取file内容
const string filename = GetFullPathOrDie(basename);
std::ifstream stream(filename.c_str()); //文件输入流
return string((std::istreambuf_iterator<char>(stream)),//使用迭代器初始化string对象
std::istreambuf_iterator<char>());
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/configuration_file_resolver.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
#define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
namespace cartographer {
namespace common {
/*
ConfigurationFileResolver是配置文件解析类,继承自FileResolver类
在给定的目录下搜索配置文件,
*/
// A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk.
// It searches the 'configuration_files_directories' in order to find the
// requested filename. The last place searched is always the
// 'configuration_files/' directory installed with Cartographer. It contains
// reasonable configuration for the various Cartographer components which
// provide a good starting ground for new platforms.
class ConfigurationFileResolver : public FileResolver {
public:
explicit ConfigurationFileResolver(
const std::vector<string>& configuration_files_directories);
string GetFullPathOrDie(const string& basename) override;//获取文件的全部路径
string GetFileContentOrDie(const string& basename) override; //获取文件内容
private:
std::vector<string> configuration_files_directories_;
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
================================================
FILE: common/fixed_ratio_sampler.cc
================================================
#include "cartographer/common/fixed_ratio_sampler.h"
namespace cartographer {
namespace common {
FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {}
FixedRatioSampler::~FixedRatioSampler() {}
bool FixedRatioSampler::Pulse() {
//如果当前采样率 samples/ pulses低于固定采样率,此次事件计入采样.
++num_pulses_;
if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
++num_samples_;
return true;
}
return false;
}
string FixedRatioSampler::DebugString() {
return std::to_string(num_samples_) + " (" +
std::to_string(100. * num_samples_ / num_pulses_) + "%)";
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/fixed_ratio_sampler.h
================================================
#ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
#define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
#include <string>
#include "cartographer/common/port.h"
namespace cartographer {
namespace common {
/*
FixedRatioSampler是频率固定的采样器类，目的是从数据流中均匀的按照固定频率采样数据
FixedRatioSampler不可拷贝,不可赋值.
成员函数提供2种操作:
Pulse()产生一个事件pulses,并且:如果计入采样samples，返回true
DebugString():以string形式输出采样率
*/
// Signals when a sample should be taken from a stream of data to select a
// uniformly distributed fraction of the data.
class FixedRatioSampler {
public:
explicit FixedRatioSampler(double ratio);
~FixedRatioSampler();
FixedRatioSampler(const FixedRatioSampler&) = delete;
FixedRatioSampler& operator=(const FixedRatioSampler&) = delete;
// Returns true if this pulse should result in an sample.
bool Pulse();
// Returns a debug string describing the current ratio of samples to pulses.
string DebugString();
private:
// Sampling occurs if the proportion of samples to pulses drops below this
// number.
const double ratio_;
int64 num_pulses_ = 0; //产生的脉冲次数
int64 num_samples_ = 0;//记录的采样次数
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
================================================
FILE: common/fixed_ratio_sampler_test.cc
================================================
#include "cartographer/common/fixed_ratio_sampler.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace common {
namespace {
TEST(FixedRatioSamplerTest, AlwaysTrue) {
FixedRatioSampler fixed_ratio_sampler(1.);//固定采样率是1hz，每次Pulse都采集samples
for (int i = 0; i < 100; ++i) {
EXPECT_TRUE(fixed_ratio_sampler.Pulse());
}
}
TEST(FixedRatioSamplerTest, AlwaysFalse) {
FixedRatioSampler fixed_ratio_sampler(0.);//0 hz，不采集samples
for (int i = 0; i < 100; ++i) {
EXPECT_FALSE(fixed_ratio_sampler.Pulse());
}
}
TEST(FixedRatioSamplerTest, SometimesTrue) {
FixedRatioSampler fixed_ratio_sampler(0.5); //0.5hz
for (int i = 0; i < 100; ++i) {
EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse());//每2次Pulse采集一次samples
}
}
TEST(FixedRatioSamplerTest, FirstPulseIsTrue) {
// Choose a very very small positive number for the ratio.
FixedRatioSampler fixed_ratio_sampler(1e-20); //0.000000...001hz
EXPECT_TRUE(fixed_ratio_sampler.Pulse());
for (int i = 0; i < 100; ++i) {
EXPECT_FALSE(fixed_ratio_sampler.Pulse()); //每100000000次Pulse采集一次samples
}
}
} // namespace
} // namespace common
} // namespace cartographer
================================================
FILE: common/histogram.cc
================================================
#include "cartographer/common/histogram.h"
#include <algorithm>
#include <numeric>
#include <string>
#include "cartographer/common/port.h"
#include "glog/logging.h"
namespace cartographer {
namespace common {
//添加元素
void Histogram::Add(const float value) { values_.push_back(value); }
//以bin大小是buckets转成 string,输出 直方图信息.
string Histogram::ToString(const int buckets) const {
CHECK_GE(buckets, 1); //篮子个数必须>=1
if (values_.empty()) {
return "Count: 0";
}
const float min = *std::min_element(values_.begin(), values_.end());
const float max = *std::max_element(values_.begin(), values_.end());
const float mean =
std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size();
string result = "Count: " + std::to_string(values_.size()) +
" Min: " + std::to_string(min) +
" Max: " + std::to_string(max) +
" Mean: " + std::to_string(mean);
if (min == max) { //最大等于最小直接返回.
return result;
}
CHECK_LT(min, max);
float lower_bound = min; //bin区间下界
int total_count = 0;
for (int i = 0; i != buckets; ++i) { //根据bin个数
const float upper_bound = //bin区间上界
(i + 1 == buckets)
? max
: (max * (i + 1) / buckets + min * (buckets - i - 1) / buckets);
int count = 0;
for (const float value : values_) { //对每一个vector<float>的值
if (lower_bound <= value &&
(i + 1 == buckets ? value <= upper_bound : value < upper_bound)) {
++count;
}
}
total_count += count;
result += "\n[" + std::to_string(lower_bound) + ", " +
std::to_string(upper_bound) + ((i + 1 == buckets) ? "]" : ")");
constexpr int kMaxBarChars = 20;
const int bar =
(count * kMaxBarChars + values_.size() / 2) / values_.size();
result += "\t";
for (int i = 0; i != kMaxBarChars; ++i) {
result += (i < (kMaxBarChars - bar)) ? " " : "#";
}
result += "\tCount: " + std::to_string(count) + " (" +
std::to_string(count * 1e2f / values_.size()) + "%)";
result += "\tTotal: " + std::to_string(total_count) + " (" +
std::to_string(total_count * 1e2f / values_.size()) + "%)";
lower_bound = upper_bound;
}
return result;
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/histogram.h
================================================
#ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_
#define CARTOGRAPHER_COMMON_HISTOGRAM_H_
#include <map>
#include <string>
#include <vector>
#include "cartographer/common/port.h"
namespace cartographer {
namespace common {
/*
Histogram直方图类,
提供2个操作:
1，Add()添加元素
2，ToString(buckets )以字符串的形式输出buckets个直方图信息,bin大小是篮子个数.
Histogram只有一个数据成员，用vector<float>表示
*/
class Histogram {
public:
void Add(float value); //添加value,可乱序
string ToString(int buckets) const;//分为几块
private:
std::vector<float> values_;
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_
/*测试：
若bin为2:[2,1,2,4,5,5]:
Count: 6 Min: 1.000000 Max: 5.000000 Mean: 3.166667
[1.000000, 3.000000) ########## Count: 3 (50.000000%) Total: 3 (50.000000%)
[3.000000, 5.000000] ########## Count: 3 (50.000000%) Total: 6 (100.000000%)
若bin为3:[2,1,2,4,5,5,6,7,8]:
Count: 10 Min: 1.000000 Max: 8.000000 Mean: 4.500000
[1.000000, 3.333333) ###### Count: 3 (30.000000%) Total: 3 (30.000000%)
[3.333333, 5.666667) ######## Count: 4 (40.000000%) Total: 7 (70.000000%)
[5.666667, 8.000000] ###### Count: 3 (30.000000%) Total: 10 (100.000000%)
*/
================================================
FILE: common/lua.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_LUA_H_
#define CARTOGRAPHER_COMMON_LUA_H_
#include <lua.hpp>
#endif // CARTOGRAPHER_COMMON_LUA_H_
================================================
FILE: common/lua_parameter_dictionary.cc
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/*LuaParameterDictionary构造函数启动一个lua解释器,此解释器运行lua code,
并返回一个key/val键值表的lua Table
*/
// When a LuaParameterDictionary is constructed, a new Lua state (i.e. an
// independent Lua interpreter) is fired up to evaluate the Lua code. The code
// is expected to return a Lua table that contains key/value pairs that are the
// key/value pairs of our parameter dictionary.
//
/*
keep around:继续运行,lua table保持在栈上,c++引用这个table,而不是加载到c++ 的map中去.
*/
// We keep the Lua interpreter around and the table on the stack and reference
// it in the Get* methods instead of moving its contents from Lua into a C++ map
// since we can only know in the Get* methods what data type to expect in the
// table.
//
// Some of the methods below documentation the current stack with the following
// notation: S: <bottom> ... <top>
/*
以下文档中的stack表示:<bottom> ..<top>
lua中的
*/
#include "cartographer/common/lua_parameter_dictionary.h"
#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
namespace cartographer {
namespace common {
namespace {
/*
lua_isstring()):当给定索引的值是一个字符串或是一个数字（数字总能转换成字符串）时，返回 1 ，否则返回 0 。
lua_gettop()返回栈顶元素的索引。因为索引是从 1 开始编号的，所以这个结果等于堆栈上的元素个数（因此返回 0 表示堆栈为空）
*/
// Replace the string at the top of the stack through a quoted version that Lua
// can read back.
void QuoteStringOnStack(lua_State* L) { //加""引号,方便lua回调
CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value.";
int current_index = lua_gettop(L);
// S: ... string
lua_pushglobaltable(L); // S: ... string globals
lua_getfield(L, -1, "string"); // S: ... string globals <string module>
lua_getfield(L, -1,
"format"); // S: ... string globals <string module> format
lua_pushstring(L, "%q"); // S: ... string globals <string module> format "%q"
lua_pushvalue(L, current_index); // S: ... string globals <string module>
// format "%q" string
lua_call(L, 2, 1); // S: ... string globals <string module> quoted
lua_replace(L, current_index); // S: ... quoted globals <string module>
lua_pop(L, 2); // S: ... quoted
}
// Sets the given 'dictionary' as the value of the "this" key in Lua's registry
// table.
void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) {
lua_pushstring(L, "this");
lua_pushlightuserdata(L, dictionary);
lua_settable(L, LUA_REGISTRYINDEX);
}
// Gets the 'dictionary' from the "this" key in Lua's registry table.
LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
lua_pushstring(L, "this");
lua_gettable(L, LUA_REGISTRYINDEX);
void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1);
lua_pop(L, 1);
CHECK(return_value != nullptr);
return reinterpret_cast<LuaParameterDictionary*>(return_value);
}
// CHECK()s if a Lua method returned an error.
void CheckForLuaErrors(lua_State* L, int status) {
CHECK_EQ(status, 0) << lua_tostring(L, -1);
}
// Returns 'a' if 'condition' is true, else 'b'.
int LuaChoose(lua_State* L) {
CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b).";
CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value.";
const bool condition = lua_toboolean(L, 1);
if (condition) {
lua_pushvalue(L, 2);
} else {
lua_pushvalue(L, 3);
}
return 1;
}
// Pushes a value to the Lua stack.
void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
void PushValue(lua_State* L, const string& key) {
lua_pushstring(L, key.c_str());
}
// Reads the value with the given 'key' from the Lua dictionary and pushes it to
// the top of the stack.
template <typename T>
void GetValueFromLuaTable(lua_State* L, const T& key) {
PushValue(L, key);
lua_rawget(L, -2);
}
// CHECK() that the topmost parameter on the Lua stack is a table.
void CheckTableIsAtTopOfStack(lua_State* L) {
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
}
// Returns true if 'key' is in the table at the top of the Lua stack.
template <typename T>
bool HasKeyOfType(lua_State* L, const T& key) {
CheckTableIsAtTopOfStack(L);
PushValue(L, key);
lua_rawget(L, -2);
const bool key_not_found = lua_isnil(L, -1);
lua_pop(L, 1); // Pop the item again.
return !key_not_found;
}
// Iterates over the integer keys of the table at the top of the stack of 'L•
// and pushes the values one by one. 'pop_value' is expected to pop a value and
// put them into a C++ container.
void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
int idx = 1;
while (true) {
GetValueFromLuaTable(L, idx);
if (lua_isnil(L, -1)) {
lua_pop(L, 1);
break;
}
pop_value();
++idx;
}
}
} // namespace
std::unique_ptr<LuaParameterDictionary>
LuaParameterDictionary::NonReferenceCounted(
const string& code, std::unique_ptr<FileResolver> file_resolver) {
return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
code, ReferenceCount::NO, std::move(file_resolver)));
}
LuaParameterDictionary::LuaParameterDictionary(
const string& code, std::unique_ptr<FileResolver> file_resolver)
: LuaParameterDictionary(code, ReferenceCount::YES,
std::move(file_resolver)) {}
LuaParameterDictionary::LuaParameterDictionary(
const string& code, ReferenceCount reference_count,
std::unique_ptr<FileResolver> file_resolver)
: L_(luaL_newstate()),
index_into_reference_table_(-1),
file_resolver_(std::move(file_resolver)),
reference_count_(reference_count) {
CHECK_NOTNULL(L_);
SetDictionaryInRegistry(L_, this);
luaL_openlibs(L_);
lua_register(L_, "choose", LuaChoose);
lua_register(L_, "include", LuaInclude);
lua_register(L_, "read", LuaRead);
CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
CheckTableIsAtTopOfStack(L_);
}
LuaParameterDictionary::LuaParameterDictionary(
lua_State* const L, ReferenceCount reference_count,
std::shared_ptr<FileResolver> file_resolver)
: L_(lua_newthread(L)),
file_resolver_(std::move(file_resolver)),
reference_count_(reference_count) {
CHECK_NOTNULL(L_);
// Make sure this is never garbage collected.
CHECK(lua_isthread(L, -1));
index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX);
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
lua_xmove(L, L_, 1); // Moves the table and the coroutine over.
CheckTableIsAtTopOfStack(L_);
}
LuaParameterDictionary::~LuaParameterDictionary() {
if (reference_count_ == ReferenceCount::YES) {
CheckAllKeysWereUsedExactlyOnceAndReset();
}
if (index_into_reference_table_ > 0) {
luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_);
} else {
lua_close(L_);
}
}
std::vector<string> LuaParameterDictionary::GetKeys() const {
CheckTableIsAtTopOfStack(L_);
std::vector<string> keys;
lua_pushnil(L_); // Push the first key
while (lua_next(L_, -2) != 0) {
lua_pop(L_, 1); // Pop value, keep key.
if (!lua_isnumber(L_, -1)) {
keys.emplace_back(lua_tostring(L_, -1));
}
}
return keys;
}
bool LuaParameterDictionary::HasKey(const string& key) const {
return HasKeyOfType(L_, key);
}
string LuaParameterDictionary::GetString(const string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopString(Quoted::NO);
}
string LuaParameterDictionary::PopString(Quoted quoted) const {
CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
if (quoted == Quoted::YES) {
QuoteStringOnStack(L_);
}
const string value = lua_tostring(L_, -1);
lua_pop(L_, 1);
return value;
}
double LuaParameterDictionary::GetDouble(const string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopDouble();
}
double LuaParameterDictionary::PopDouble() const {
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
const double value = lua_tonumber(L_, -1);
lua_pop(L_, 1);
return value;
}
int LuaParameterDictionary::GetInt(const string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopInt();
}
int LuaParameterDictionary::PopInt() const {
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
const int value = lua_tointeger(L_, -1);
lua_pop(L_, 1);
return value;
}
bool LuaParameterDictionary::GetBool(const string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopBool();
}
bool LuaParameterDictionary::PopBool() const {
CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value.";
const bool value = lua_toboolean(L_, -1);
lua_pop(L_, 1);
return value;
}
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
const string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopDictionary(reference_count_);
}
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
ReferenceCount reference_count) const {
CheckTableIsAtTopOfStack(L_);
std::unique_ptr<LuaParameterDictionary> value(
new LuaParameterDictionary(L_, reference_count, file_resolver_));
// The constructor lua_xmove()s the value, no need to pop it.
CheckTableIsAtTopOfStack(L_);
return value;
}
string LuaParameterDictionary::DoToString(const string& indent) const {
string result = "{";
bool dictionary_is_empty = true;
const auto top_of_stack_to_string = [this, indent,
&dictionary_is_empty]() -> string {
dictionary_is_empty = false;
const int value_type = lua_type(L_, -1);
switch (value_type) {
case LUA_TBOOLEAN:
return PopBool() ? "true" : "false";
break;
case LUA_TSTRING:
return PopString(Quoted::YES);
break;
case LUA_TNUMBER: {
const double value = PopDouble();
if (std::isinf(value)) {
return value < 0 ? "-math.huge" : "math.huge";
} else {
return std::to_string(value);
}
} break;
case LUA_TTABLE: {
std::unique_ptr<LuaParameterDictionary> subdict(
PopDictionary(ReferenceCount::NO));
return subdict->DoToString(indent + " ");
} break;
default:
LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type);
}
};
// Integer (array) keys.
for (int i = 1; i; ++i) {
GetValueFromLuaTable(L_, i);
if (lua_isnil(L_, -1)) {
lua_pop(L_, 1);
break;
}
result.append("\n");
result.append(indent);
result.append(" ");
result.append(top_of_stack_to_string());
result.append(",");
}
// String keys.
std::vector<string> keys = GetKeys();
if (!keys.empty()) {
std::sort(keys.begin(), keys.end());
for (const string& key : keys) {
GetValueFromLuaTable(L_, key);
result.append("\n");
result.append(indent);
result.append(" ");
result.append(key);
result.append(" = ");
result.append(top_of_stack_to_string());
result.append(",");
}
}
result.append("\n");
result.append(indent);
result.append("}");
if (dictionary_is_empty) {
return "{}";
}
return result;
}
string LuaParameterDictionary::ToString() const { return DoToString(""); }
std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
std::vector<double> values;
GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); });
return values;
}
std::vector<std::unique_ptr<LuaParameterDictionary>>
LuaParameterDictionary::GetArrayValuesAsDictionaries() {
std::vector<std::unique_ptr<LuaParameterDictionary>> values;
GetArrayValues(L_, [&values, this] {
values.push_back(PopDictionary(reference_count_));
});
return values;
}
std::vector<string> LuaParameterDictionary::GetArrayValuesAsStrings() {
std::vector<string> values;
GetArrayValues(L_,
[&values, this] { values.push_back(PopString(Quoted::NO)); });
return values;
}
void LuaParameterDictionary::CheckHasKey(const string& key) const {
CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
<< ToString();
}
void LuaParameterDictionary::CheckHasKeyAndReference(const string& key) {
CheckHasKey(key);
reference_counts_[key]++;
}
void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
for (const auto& key : GetKeys()) {
CHECK_EQ(1, reference_counts_.count(key))
<< "Key '" << key << "' was used the wrong number of times.";
CHECK_EQ(1, reference_counts_.at(key))
<< "Key '" << key << "' was used the wrong number of times.";
}
reference_counts_.clear();
}
int LuaParameterDictionary::GetNonNegativeInt(const string& key) {
const int temp = GetInt(key); // Will increase reference count.
CHECK_GE(temp, 0) << temp << " is negative.";
return temp;
}
// Lua function to run a script in the current Lua context. Takes the filename
// as its only argument.
int LuaParameterDictionary::LuaInclude(lua_State* L) {
CHECK_EQ(lua_gettop(L), 1);
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
const string basename = lua_tostring(L, -1);
const string filename =
parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
if (std::find(parameter_dictionary->included_files_.begin(),
parameter_dictionary->included_files_.end(),
filename) != parameter_dictionary->included_files_.end()) {
string error_msg = "Tried to include " + filename +
" twice. Already included files in order of inclusion: ";
for (const string& filename : parameter_dictionary->included_files_) {
error_msg.append(filename);
error_msg.append("\n");
}
LOG(FATAL) << error_msg;
}
parameter_dictionary->included_files_.push_back(filename);
lua_pop(L, 1);
CHECK_EQ(lua_gettop(L), 0);
const string content =
parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
CheckForLuaErrors(
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
return lua_gettop(L);
}
// Lua function to read a file into a string.
int LuaParameterDictionary::LuaRead(lua_State* L) {
CHECK_EQ(lua_gettop(L), 1);
CHECK(lua_isstring(L, -1)) << "read takes a filename.";
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
const string file_content =
parameter_dictionary->file_resolver_->GetFileContentOrDie(
lua_tostring(L, -1));
lua_pushstring(L, file_content.c_str());
return 1;
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/lua_parameter_dictionary.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
#include <map>
#include <memory>
#include <string>
#include <vector>
#include "cartographer/common/lua.h"
#include "cartographer/common/port.h"
#include "glog/logging.h"
namespace cartographer {
namespace common {
/*
FileResolver是一个文件解析的虚基类,利用lua加载文件内容
*/
// Resolves file paths and file content for the Lua 'read' and 'include'
// functions. Use this to configure where those functions load other files from.
class FileResolver {
public:
virtual ~FileResolver() {}
virtual string GetFullPathOrDie(const string& basename) = 0;
virtual string GetFileContentOrDie(const string& basename) = 0;
};
/*
LuaParameterDictionary类继承自FileResolver,
目的是从lua配置文件加载参数.该类不可拷贝不可赋值.
1,构造函数从file_resolver中加载一个带lua table的字典dictionary
2,
code是以字符串代表的lua代码code
*/
// A parameter dictionary that gets loaded from Lua code.
class LuaParameterDictionary {
public:
// Constructs the dictionary from a Lua Table specification.
LuaParameterDictionary(const string& code,
std::unique_ptr<FileResolver> file_resolver);
LuaParameterDictionary(const LuaParameterDictionary&) = delete;
LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
// Constructs a LuaParameterDictionary without reference counting.
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
const string& code, std::unique_ptr<FileResolver> file_resolver);
~LuaParameterDictionary();
// Returns all available keys.
std::vector<string> GetKeys() const;
// Returns true if the key is in this dictionary.
bool HasKey(const string& key) const;
// These methods CHECK() that the 'key' exists.
string GetString(const string& key);
double GetDouble(const string& key);
int GetInt(const string& key);
bool GetBool(const string& key);
std::unique_ptr<LuaParameterDictionary> GetDictionary(const string& key);
// Gets an int from the dictionary and CHECK()s that it is non-negative.
int GetNonNegativeInt(const string& key);
// Returns a string representation for this LuaParameterDictionary.
string ToString() const;
// Returns the values of the keys '1', '2', '3' as the given types.
std::vector<double> GetArrayValuesAsDoubles();
std::vector<string> GetArrayValuesAsStrings();
std::vector<std::unique_ptr<LuaParameterDictionary>>
GetArrayValuesAsDictionaries();
private:
enum class ReferenceCount { YES, NO };
LuaParameterDictionary(const string& code, ReferenceCount reference_count,
std::unique_ptr<FileResolver> file_resolver);
// For GetDictionary().
LuaParameterDictionary(lua_State* L, ReferenceCount reference_count,
std::shared_ptr<FileResolver> file_resolver);
// Function that recurses to keep track of indent for ToString().
string DoToString(const string& indent) const;
// Pop the top of the stack and CHECKs that the type is correct.
double PopDouble() const;
int PopInt() const;
bool PopBool() const;
// Pop the top of the stack and CHECKs that it is a string. The returned value
// is either quoted to be suitable to be read back by a Lua interpretor or
// not.
enum class Quoted { YES, NO };
string PopString(Quoted quoted) const;
// Creates a LuaParameterDictionary from the Lua table at the top of the
// stack, either with or without reference counting.
std::unique_ptr<LuaParameterDictionary> PopDictionary(
ReferenceCount reference_count) const;
// CHECK() that 'key' is in the dictionary.
void CheckHasKey(const string& key) const;
// CHECK() that 'key' is in this dictionary and reference it as being used.
void CheckHasKeyAndReference(const string& key);
// If desired, this can be called in the destructor of a derived class. It
// will CHECK() that all keys defined in the configuration have been used
// exactly once and resets the reference counter.
void CheckAllKeysWereUsedExactlyOnceAndReset();
// Reads a file into a Lua string.
static int LuaRead(lua_State* L);
// Handles inclusion of other Lua files and prevents double inclusion.
static int LuaInclude(lua_State* L);
lua_State* L_; // The name is by convention in the Lua World.
int index_into_reference_table_;
// This is shared with all the sub dictionaries.
const std::shared_ptr<FileResolver> file_resolver_;
// If true will check that all keys were used on destruction.
const ReferenceCount reference_count_;
// This is modified with every call to Get* in order to verify that all
// parameters are read exactly once.
std::map<string, int> reference_counts_;
// List of all included files in order of inclusion. Used to prevent double
// inclusion.
std::vector<string> included_files_;
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
================================================
FILE: common/lua_parameter_dictionary_test.cc
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/common/lua_parameter_dictionary.h"
#include <algorithm>
#include <cmath>
#include <memory>
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace common {
namespace {
/*返回一个智能指针,指向DummyFileResolver*/
std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
const string& code) {
return LuaParameterDictionary::NonReferenceCounted(
code, common::make_unique<DummyFileResolver>());
}
class LuaParameterDictionaryTest : public ::testing::Test {
protected:
void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
for (const string& key : dict->GetKeys()) {
dict->GetInt(key);
}
}
};
TEST_F(LuaParameterDictionaryTest, GetInt) {
auto dict = MakeDictionary("return { blah = 100 }");
ASSERT_EQ(dict->GetInt("blah"), 100); // blah==100
}
TEST_F(LuaParameterDictionaryTest, GetString) {
auto dict = MakeDictionary("return { blah = 'is_a_string' }\n");
ASSERT_EQ(dict->GetString("blah"), "is_a_string");//blah = 'is_a_string'
}
TEST_F(LuaParameterDictionaryTest, GetDouble) {
auto dict = MakeDictionary("return { blah = 3.1415 }");
ASSERT_DOUBLE_EQ(dict->GetDouble("blah"), 3.1415); //blah = 3.1415
}
TEST_F(LuaParameterDictionaryTest, GetBoolTrue) {
auto dict = MakeDictionary("return { blah = true }");
ASSERT_TRUE(dict->GetBool("blah")); //blah = true
}
TEST_F(LuaParameterDictionaryTest, GetBoolFalse) {
auto dict = MakeDictionary("return { blah = false }");
ASSERT_FALSE(dict->GetBool("blah")); // blah = false
}
TEST_F(LuaParameterDictionaryTest, GetDictionary) {
auto dict =
MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }");
//sub_dict 是一个子字典
std::unique_ptr<LuaParameterDictionary> sub_dict(dict->GetDictionary("blah"));
std::vector<string> keys = sub_dict->GetKeys();//blah下的key
ASSERT_EQ(keys.size(), 2); //2个key,blue,red
std::sort(keys.begin(), keys.end()); //顺序不定
ASSERT_EQ(keys[0], "blue");
ASSERT_EQ(keys[1], "red");
ASSERT_TRUE(sub_dict->HasKey("blue"));
ASSERT_TRUE(sub_dict->HasKey("red"));
ASSERT_EQ(sub_dict->GetInt("blue"), 100); //blue==100
ASSERT_EQ(sub_dict->GetInt("red"), 200); //red==10
ASSERT_EQ(dict->GetString("fasel"), "10");
}
TEST_F(LuaParameterDictionaryTest, GetKeys) {
auto dict = MakeDictionary("return { blah = 100, blah1 = 200}");
std::vector<string> keys = dict->GetKeys(); //返回key组成的string
ASSERT_EQ(keys.size(), 2);
std::sort(keys.begin(), keys.end());
ASSERT_EQ(keys[0], "blah");
ASSERT_EQ(keys[1], "blah1");
ReferenceAllKeysAsIntegers(dict.get());
}
TEST_F(LuaParameterDictionaryTest, NonExistingKey) {
auto dict = MakeDictionary("return { blah = 100 }");
ReferenceAllKeysAsIntegers(dict.get());
ASSERT_DEATH(dict->GetInt("blah_fasel"), "Key.* not in dictionary.");//key不存在
}
TEST_F(LuaParameterDictionaryTest, UintNegative) {
auto dict = MakeDictionary("return { blah = -100}");
ASSERT_DEATH(dict->GetNonNegativeInt("blah"), ".*-100 is negative.");//key对应的value是负数
ReferenceAllKeysAsIntegers(dict.get());
}
TEST_F(LuaParameterDictionaryTest, ToString) {
auto dict = MakeDictionary(R"(return {
ceta = { yolo = "hurray" },
fasel = 1234.456786,
fasel1 = -math.huge,
fasel2 = math.huge,
blubber = 123,
blub = 'hello',
alpha = true,
alpha1 = false,
})");
const string golden = R"({
alpha = true,
alpha1 = false,
blub = "hello",
blubber = 123.000000,
ceta = {
yolo = "hurray",
},
fasel = 1234.456786,
fasel1 = -math.huge,
fasel2 = math.huge,
})";
EXPECT_EQ(golden, dict->ToString()); //相等,dict 开始的return 不包含在内
auto dict1 = MakeDictionary("return " + dict->ToString());
EXPECT_EQ(dict1->GetBool("alpha"), true);
EXPECT_EQ(dict1->GetBool("alpha1"), false);
EXPECT_EQ(dict1->GetInt("blubber"), 123); //获取整数
EXPECT_EQ(dict1->GetString("blub"), "hello");
EXPECT_EQ(dict1->GetDictionary("ceta")->GetString("yolo"), "hurray");
EXPECT_NEAR(dict1->GetDouble("fasel"), 1234.456786, 1e-3);
EXPECT_TRUE(std::isinf(-dict1->GetDouble("fasel1")));
EXPECT_TRUE(std::isinf(dict1->GetDouble("fasel2")));
EXPECT_EQ(dict->GetBool("alpha"), true);
EXPECT_EQ(dict->GetBool("alpha1"), false);
EXPECT_EQ(dict->GetInt("blubber"), 123);
EXPECT_EQ(dict->GetString("blub"), "hello");
EXPECT_EQ(dict->GetDictionary("ceta")->GetString("yolo"), "hurray");
EXPECT_NEAR(dict->GetDouble("fasel"), 1234.456786, 1e-3);
EXPECT_TRUE(std::isinf(-dict->GetDouble("fasel1")));
EXPECT_TRUE(std::isinf(dict->GetDouble("fasel2")));
}
TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
auto dict = MakeNonReferenceCounted(
R"(return {
"blub", 3, 3.1,
foo = "ups",
})");
const string golden = R"({
"blub",
3.000000,
3.100000,
foo = "ups",
})";
EXPECT_EQ(golden, dict->ToString());
}
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
auto dict = MakeDictionary("return { 'a', 'b', 'c' }");
EXPECT_EQ(0, dict->GetKeys().size());
const std::vector<string> values = dict->GetArrayValuesAsStrings();
EXPECT_EQ(3, values.size());
EXPECT_EQ("a", values[0]);
EXPECT_EQ("b", values[1]);
EXPECT_EQ("c", values[2]);
}
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) {
auto dict = MakeDictionary("return { 1., 2., 3. }");
EXPECT_EQ(0, dict->GetKeys().size());
const std::vector<double> values = dict->GetArrayValuesAsDoubles();
EXPECT_EQ(3, values.size());
EXPECT_NEAR(1., values[0], 1e-7);
EXPECT_NEAR(2., values[1], 1e-7);
EXPECT_NEAR(3., values[2], 1e-7);
}
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) {
auto dict = MakeDictionary("return { { a = 1 }, { b = 3 } }");
EXPECT_EQ(0, dict->GetKeys().size());
const std::vector<std::unique_ptr<LuaParameterDictionary>> values =
dict->GetArrayValuesAsDictionaries();
EXPECT_EQ(2, values.size());
EXPECT_EQ(1., values[0]->GetInt("a"));
EXPECT_EQ(3., values[1]->GetInt("b"));
}
TEST_F(LuaParameterDictionaryTest, TestChooseTrue) {
auto dict = MakeDictionary("return { a = choose(true, 1, 0) }");
EXPECT_EQ(1, dict->GetInt("a"));
}
TEST_F(LuaParameterDictionaryTest, TestChoseFalse) {
auto dict = MakeDictionary("return { a = choose(false, 1, 0) }");
EXPECT_EQ(0, dict->GetInt("a"));
}
TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) {
EXPECT_DEATH(MakeDictionary("return { a = choose('truish', 1, 0) }"),
"condition is not a boolean value.");
}
} // namespace
} // namespace common
} // namespace cartographer
================================================
FILE: common/lua_parameter_dictionary_test_helpers.h
================================================
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
#include <memory>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "glog/logging.h"
namespace cartographer {
namespace common {
class DummyFileResolver : public FileResolver {
public:
DummyFileResolver() {}
DummyFileResolver(const DummyFileResolver&) = delete;
DummyFileResolver& operator=(const DummyFileResolver&) = delete;
~DummyFileResolver() override {}
string GetFileContentOrDie(const string& unused_basename) override {
LOG(FATAL) << "Not implemented";
}
string GetFullPathOrDie(const string& unused_basename) override {
LOG(FATAL) << "Not implemented";
}
};
std::unique_ptr<LuaParameterDictionary> MakeDictionary(const string& code) {
return common::make_unique<LuaParameterDictionary>(
code, common::make_unique<DummyFileResolver>());
}
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
================================================
FILE: common/make_unique.h
================================================
#ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
#define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
#include <cstddef>
#include <memory>
#include <type_traits>
#include <utility>
namespace cartographer {
namespace common {
/*
make_unique.h在不支持c++14的环境下实现 std::make_unique的功能
实现细节:完美转发和移动语义
*/
// Implementation of c++14's std::make_unique, taken from
// https://isocpp.org/files/papers/N3656.txt
template <class T>
struct _Unique_if {
typedef std::unique_ptr<T> _Single_object;
};
template <class T>
struct _Unique_if<T[]> {
typedef std::unique_ptr<T[]> _Unknown_bound; //不支持数组(不定长)
};
template <class T, size_t N>
struct _Unique_if<T[N]> { //不支持数组(定长)
typedef void _Known_bound;
};
template <class T, class... Args>
typename _Unique_if<T>::_Single_object make_unique(Args&&... args) {
return std::unique_ptr<T>(new T(std::forward<Args>(args)...)); //完美转发参数
}
template <class T>
typename _Unique_if<T>::_Unknown_bound make_unique(size_t n) {
typedef typename std::remove_extent<T>::type U;
return std::unique_ptr<T>(new U[n]());
}
template <class T, class... Args>
typename _Unique_if<T>::_Known_bound make_unique(Args&&...) = delete;//不能使用定长数组
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
/*
完美转发:
完美的传递函数参数而不修改参数类型，左值依然是左值，右值依然是右值。
*/
================================================
FILE: common/make_unique_test.cc
================================================
#include <iostream>
#include <string>
#include "cartographer/common/make_unique.h"
#include "gtest/gtest.h"
//using namespace std;
using std::cout;
using std::cin;
using std::endl;
using std::string;
namespace cartographer {
namespace common {
TEST(MAKE_UNIQUE ,make_unique) {
cout << *make_unique<int>() << endl;
cout << *make_unique<int>(1729) << endl;
cout << "\"" << *make_unique<string>() << "\"" << endl;
cout << "\"" << *make_unique<string>("meow") << "\"" << endl;
cout << "\"" << *make_unique<string>(6, 'z') << "\"" << endl;
auto up = make_unique<int[]>(5);
for (int i = 0; i < 5; ++i) {
cout << up[i] << " ";
}
cout << endl;
#if defined(ERROR1)
auto up1 = make_unique<string[]>("error");
#elif defined(ERROR2)
auto up2 = make_unique<int[]>(10, 20, 30, 40);
#elif defined(ERROR3)
auto up3 = make_unique<int[5]>();
#elif defined(ERROR4)
auto up4 = make_unique<int[5]>(11, 22, 33, 44, 55);
#endif
}
/*
输出:
0
1729
""
"meow"
"zzzzzz"
0 0 0 0 0
*/
}
}
================================================
FILE: common/math.h
================================================
/*
common/math.h文件主要实现数学计算，包括：
区间截断.求n次方.求平方.幅度角度转换.归一化.反正切值
*/
#ifndef CARTOGRAPHER_COMMON_MATH_H_
#define CARTOGRAPHER_COMMON_MATH_H_
#include <cmath>
#include <vector>
#include "Eigen/Core"
#include "cartographer/common/port.h"
#include "ceres/ceres.h"
namespace cartographer {
namespace common {
// Clamps 'value' to be in the range ['min', 'max'].
//将val截取到区间min至max中.
template <typename T>
T Clamp(const T value, const T min, const T max) {
if (value > max) {
return max;
}
if (value < min) {
return min;
}
return value;
}
// Calculates 'base'^'exponent'. 计算base的exp次方
template <typename T>
constexpr T Power(T base, int exponent) {
return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
}
// Calculates a^2. 特化,求平方
template <typename T>
constexpr T Pow2(T a) {
return Power(a, 2);
}
// Converts from degrees to radians.角度到弧度的转换. 60° -> pi/3
constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
// Converts form radians to degrees.弧度到角度的转换, pi/3 -> 60°
constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
// Bring the 'difference' between two angles into [-pi; pi]
//将角度差转换为[-pi;pi]
template <typename T>
T NormalizeAngleDifference(T difference) {
while (difference > M_PI) {
difference -= T(2. * M_PI);
}
while (difference < -M_PI) {
difference += T(2. * M_PI);
}
return difference;
}
/*
atan2 返回原点至点(x,y)的方位角，即与 x 轴的夹角，
也可以理解为计算复数 x+yi 的辐角,范围是[-pi,pi]
ATAN2(1,1) -> pi/4:以弧度表示点(1,1)的反正切值，即pi/4(0.785398)
*/
template <typename T>
T atan2(const Eigen::Matrix<T, 2, 1>& vector) { //范围是[-pi,pi]
return ceres::atan2(vector.y(), vector.x());
}
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_MATH_H_
================================================
FILE: common/math_test.cc
================================================
#include "cartographer/common/math.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace common {
namespace {
TEST(MathTest, testPower) {
EXPECT_EQ(0., Power(0, 42));// 0的42次方==0
EXPECT_EQ(1., Power(0, 0)); //0^0 ==0
EXPECT_EQ(1., Power(1, 0)); // 1^0 ==0
EXPECT_EQ(1., Power(1, 42));//1^42==1
EXPECT_EQ(4., Power(2, 2)); //2^2==4
}
TEST(MathTest, testPow2) {
EXPECT_EQ(0., Pow2(0)); //0^2==0
EXPECT_EQ(1., Pow2(1)); //1^2==1
EXPECT_EQ(4., Pow2(2)); //2^2==4
EXPECT_EQ(49., Pow2(7));//7^2==49
}
TEST(MathTest, testDeg2rad) {
EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9); // 180° ==pi
EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6);//360° ==2pi
}
TEST(MathTest, testRad2deg) {
EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9); //pi ==180°
EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6);//2pi ==360°
}
TEST(MathTest, testNormalizeAngleDifference) {
EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9); //0==0
EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9); //pi==oi
EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9); //-pi==-pi
EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9); //2pi==0
EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9); //5pi==pi
EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9);//-5pi=-pi
}
} // namespace
} // namespace common
} // namespace cartographer
================================================
FILE: common/mutex.h
================================================
/*
common/mutex.h主要是对c++11 的mutex的封装。
*/
#ifndef CARTOGRAPHER_COMMON_MUTEX_H_
#define CARTOGRAPHER_COMMON_MUTEX_H_
#include <condition_variable>
#include <mutex>
#include "cartographer/common/time.h"
namespace cartographer {
namespace common {
/*不使用clang时,以下宏定义全部为空操作.故看见宏定义时可暂时忽略*/
// Enable thread safety attributes only with clang.
// The attributes can be safely erased when compiling with other compilers.
#if defined(__SUPPORT_TS_ANNOTATION__) || defined(__clang__)
#define THREAD_ANNOTATION_ATTRIBUTE__(x) __attribute__((x))
#else
#define THREAD_ANNOTATION_ATTRIBUTE__(x) // no-op,空操作
#endif
#define CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(capability(x))
#define SCOPED_CAPABILITY THREAD_ANNOTATION_ATTRIBUTE__(scoped_lockable)
#define GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(guarded_by(x))
#define PT_GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(pt_guarded_by(x))
#define REQUIRES(...) \
THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__))
#define ACQUIRE(...) \
THREAD_ANNOTATION_ATTRIBUTE__(acquire_capability(__VA_ARGS__))
#define RELEASE(...) \
THREAD_ANNOTATION_ATTRIBUTE__(release_capability(__VA_ARGS__))
#define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__))
#define NO_THREAD_SAFETY_ANALYSIS \
THREAD_ANNOTATION_ATTRIBUTE__(no_thread_safety_analysis)
/*
Mutex是c++11 mutex的封装，
Mutex类有一个内部类Locker。
Locker 是一个RAII的类型.
在构造Locker时,对mutex上锁,在析构Locker时对mutex解锁.
本质是使用std::unique_lock和std::condition_variable实现的
Locker类提供2个成员函数Await()和AwaitWithTimeout()
功能是利用c++11的条件变量和unique_lock实现在谓词predicate为真的情况下对mutex解锁。
*/
// Defines an annotated mutex that can only be locked through its scoped locker
// implementation.
class CAPABILITY("mutex") Mutex {
public:
// A RAII class that acquires a mutex in its constructor, and
// releases it in its destructor. It also implements waiting functionality on
// conditions that get checked whenever the mutex is released.
class SCOPED_CAPABILITY Locker {
public:
Locker(Mutex* mutex) ACQUIRE(mutex) : mutex_(mutex), lock_(mutex->mutex_) {}//上锁
~Locker() RELEASE() {
lock_.unlock(); //解锁
mutex_->condition_.notify_all();//条件变量通知解锁
}
template <typename Predicate>
void Await(Predicate predicate) REQUIRES(this) {
/*wait()实质是分3步：
1.对lock_解锁
2.等待predicate谓词为真，此时调用端阻塞。
3.对lock_ 重新上锁。
*/
mutex_->condition_.wait(lock_, predicate);
}
template <typename Predicate>
bool AwaitWithTimeout(Predicate predicate, common::Duration timeout)
REQUIRES(this) { //等待谓词为真或者直到超时timeout返回。
return mutex_->condition_.wait_for(lock_, timeout, predicate);
}
private:
Mutex* mutex_;
std::unique_lock<std::mutex> lock_;
};
private:
std::condition_variable condition_;
std::mutex mutex_;
};
using MutexLocker = Mutex::Locker;
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_MUTEX_H_
/*
谓词.断言.Predicate:
一个用于比较的函数（或函数对象），输入两个T类型，返回bool类型。
*/
================================================
FILE: 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流中，正好是压缩的逆序。
*/
================================================
FILE: common/port_le_test.cpp
================================================
#include <vector>
#include <iostream>
#include <vector>
/*
c++11 已经支持back_inserter。
*/
//#include<boost/iostreams/device/back_inserter.hpp>
using std::cout;
using std::endl;
int main(int argc, char *argv[])
{
std::vector<int> vec;
for (int i = 0 ; i < 3; ++i)
vec.push_back(i);
std::vector <int>::iterator vIter;
std::cout << "The initial vector vec is: ( ";
for (vIter = vec.begin(); vIter != vec.end(); vIter++)
std::cout << *vIter << " ";
std::cout << ")." << std::endl;
// Insertions can be done with template function
std::back_insert_iterator<std::vector<int> > backiter(vec);
*backiter = 30;
backiter++;
*backiter = 40;
// Alternatively, insertions can be done with the
// back_insert_iterator member function
std::back_inserter(vec) = 500;//从vector后插入
std::back_inserter(vec) = 600;
std::cout << "After the insertions, the vector vec is: ( ";
for (vIter = vec.begin(); vIter != vec.end(); vIter++)
std::cout << *vIter << " ";
std::cout << ")." << std::endl<<"----\n\n";
std::vector<int> firstvector, secondvector;
for (int i=1; i<=5; i++)
{
firstvector.push_back(i);
secondvector.push_back(i*10);
}
// 在firstvector向量后面加入secondvector的值.
std::copy(secondvector.begin(), secondvector.end(), std::back_inserter(firstvector));
std::vector<int>::iterator it;
for ( it = firstvector.begin(); it!= firstvector.end(); ++it )
std::cout << *it << " ";
std::cout << std::endl;
return 0;
}
================================================
FILE: common/proto/ceres_solver_options.proto
================================================
// Copyright 2016 The Cartographer Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
syntax = "proto2";
package cartographer.common.proto;
message CeresSolverOptions {
// Configure the Ceres solver. See the Ceres documentation for more
// information: https://code.google.com/p/ceres-solver/
optional bool use_nonmonotonic_steps = 1;
optional int32 max_num_iterations = 2;
optional int32 num_threads = 3;
}
================================================
FILE: common/rate_timer.h
================================================
#ifndef CARTOGRAPHER_COMMON_RATE_TIMER_H_
#define CARTOGRAPHER_COMMON_RATE_TIMER_H_
#include <chrono>
#include <deque>
#include <iomanip>
#include <numeric>
#include <sstream>
#include <string>
#include <vector>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
/*
* RateTimer是,脉冲频率计数类,计算在一段时间内的脉冲率
* RateTimer不可拷贝和赋值
* RateTimer只有一个构造函数,提供时间段Duration
* ComputeRate()返回事件脉冲率,单位hz
* ComputeWallTimeRateRatio()返回真实时间与墙上挂钟时间的比率
* 内部类Event封装了某个事件发生的时间点.
* 调用一次Pulse()即产生了一次事件
* */
namespace cartographer {
namespace common {
// Computes the rate at which pulses come in.默认模板参数是steady_clock
template <typename ClockType = std::chrono::steady_clock>
class RateTimer {
public:
// Computes the rate at which pulses come in over 'window_duration' in wall
// time.
explicit RateTimer(const common::Duration window_duration)
: window_duration_(window_duration) {}
~RateTimer() {}
RateTimer(const RateTimer&) = delete; //不可拷贝/不可赋值
RateTimer& operator=(const RateTimer&) = delete;
// Returns the pulse rate in Hz.
double ComputeRate() const { //计算频率
if (events_.empty()) {
return 0.;
}
//事件次数除以时间即为每秒钟发生多少次事件
return static_cast<double>(events_.size() - 1) /
common::ToSeconds((events_.back().time - events_.front().time));
//最晚发生的时间-最早发生的时间 (事件产生时的真实时间)
}
// Returns the ratio of the pulse rate (with supplied times) to the wall time
// rate. For example, if a sensor produces pulses at 10 Hz, but we call Pulse
// at 20 Hz wall time, this will return 2.
double ComputeWallTimeRateRatio() const { //返回比率
if (events_.empty()) {
return 0.;
}
return common::ToSeconds((events_.back().time - events_.front().time)) / //真实时间
//墙上挂钟时间,->调用Pulse时的系统的时间
std::chrono::duration_cast<std::chrono::duration<double>>(
events_.back().wall_time - events_.front().wall_time)
.count();
}
// Records an event that will contribute to the computed rate.
void Pulse(common::Time time) { //产生一个脉冲
events_.push_back(Event{time, ClockType::now()});
while (events_.size() > 2 &&
(events_.back().wall_time - events_.front().wall_time) >
window_duration_) {
events_.pop_front();
}
}
// Returns a debug string representation.
string DebugString() const {
if (events_.size() < 2) {
return "unknown";
}
std::ostringstream out;
out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz "
<< DeltasDebugString() << " (pulsed at "
<< ComputeWallTimeRateRatio() * 100. << "% real time)";
return out.str();
}
private:
struct Event {
common::Time time;
typename ClockType::time_point wall_time;
};
// Computes all differences in seconds between consecutive pulses.
std::vector<double> ComputeDeltasInSeconds() const {
CHECK_GT(events_.size(), 1);
const size_t count = events_.size() - 1;
std::vector<double> result;
result.reserve(count);
for (size_t i = 0; i != count; ++i) {
result.push_back(
common::ToSeconds(events_[i + 1].time - events_[i].time));
}
return result;
}
// Returns the average and standard deviation of the deltas.
string DeltasDebugString() const {
const auto deltas = ComputeDeltasInSeconds();
const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.);
const double mean = sum / deltas.size();
double squared_sum = 0.;
for (const double x : deltas) {
squared_sum += common::Pow2(x - mean);
}
const double sigma = std::sqrt(squared_sum / (deltas.size() - 1));
std::ostringstream out;
out << std::scientific << std::setprecision(2) << mean << " s +/- " << sigma
<< " s";
return out.str();
}
std::deque<Event> events_;
const common::Duration window_duration_;
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_RATE_TIMER_H_
================================================
FILE: common/rate_timer_test.cc
================================================
#include "cartographer/common/rate_timer.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace common {
namespace {
TEST(RateTimerTest, ComputeRate) {
RateTimer<> rate_timer(common::FromSeconds(1.));//默认模板参数是steady_clock,设置时间段是1s
common::Time time = common::FromUniversal(42); // 时间点,42us
for (int i = 0; i < 100; ++i) { //每隔0.1s产生一次事件,产生100次
rate_timer.Pulse(time);
time += common::FromSeconds(0.1);
}
EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3);//频率应该等于10次每s
}
struct SimulatedClock { /*模拟的时间clock类*/
using rep = std::chrono::steady_clock::rep;
using period = std::chrono::steady_clock::period;
using duration = std::chrono::steady_clock::duration;
using time_point = std::chrono::steady_clock::time_point;
static constexpr bool is_steady = true;
static time_point time;
static time_point now() noexcept { return time; }
};
SimulatedClock::time_point SimulatedClock::time;
TEST(RateTimerTest, ComputeWallTimeRateRatio) {
common::Time time = common::FromUniversal(42); //时间点 42us
RateTimer<SimulatedClock> rate_timer(common::FromSeconds(1.)); //时间段是1s
for (int i = 0; i < 100; ++i) { //100次事件,0.1s一次.
rate_timer.Pulse(time);
time += common::FromSeconds(0.1); //加0.1s,代表事件发生的时间(调用了Pulse)
SimulatedClock::time += //模仿的时间,加0.05s,代表流失了0.05秒.
std::chrono::duration_cast<SimulatedClock::duration>(
std::chrono::duration<double>(0.05));
}
EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); //0.1/0.05==2
}
} // namespace
} // namespace common
} // namespace cartographer
================================================
FILE: common/thread_pool.cc
================================================
#include "cartographer/common/thread_pool.h"
#include <unistd.h>
#include <algorithm>
#include <chrono>
#include <numeric>
#include "glog/logging.h"
namespace cartographer {
namespace common {
/*
pool_是vector,每个线程初始化时,执行DoWork()函数.
*/
ThreadPool::ThreadPool(int num_threads) {
MutexLocker locker(&mutex_);
for (int i = 0; i != num_threads; ++i) {
pool_.emplace_back([this]() { ThreadPool::DoWork(); });
}
}
/*
只有线程池的work_queue_=0时,才能析构,此时所有的线程已经开始被pool执行,
只有等待pool结束所有的线程执行函数(join结束),ThreadPool才能析构完成
*/
ThreadPool::~ThreadPool() {
{
MutexLocker locker(&mutex_);
CHECK(running_);
running_ = false;
CHECK_EQ(work_queue_.size(), 0);
}
for (std::thread& thread : pool_) {
thread.join();
}
}
/*
使想要线程池执行的work_item函数加入到等待队列中,然后排队等待空闲线程"领取"该函数.
*/
void ThreadPool::Schedule(std::function<void()> work_item) {
MutexLocker locker(&mutex_);
CHECK(running_);
work_queue_.push_back(work_item);
}
/*
每个线程都要执行这个函数.
ThreadPool利用条件变量通知此函数,work_queue_不为零时,说明有需要执行的函数,此时加入执行
*/
void ThreadPool::DoWork() {
#ifdef __linux__
// This changes the per-thread nice level of the current thread on Linux. We
// do this so that the background work done by the thread pool is not taking
// away CPU resources from more important foreground threads.
CHECK_NE(nice(10), -1);
#endif
for (;;) {
std::function<void()> work_item;
{
//加锁,同一时刻只能一个线程"领取"某个函数,何时执行这个函数呢?
// 最后一行即为抢到的线程开始执行work_item
MutexLocker locker(&mutex_);
locker.Await([this]() REQUIRES(mutex_) {
return !work_queue_.empty() || !running_;
//注意,队列不为空或者不在running_状态,条件变量都需要通知该函数
});
if (!work_queue_.empty()) {
work_item = work_queue_.front(); //领取
work_queue_.pop_front(); //删除
} else if (!running_) {
return;
}
}
CHECK(work_item);
work_item();
}
}
} // namespace common
} // namespace cartographer
================================================
FILE: common/thread_pool.h
================================================
#ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_
#define CARTOGRAPHER_COMMON_THREAD_POOL_H_
#include <deque>
#include <functional>
#include <thread>
#include <vector>
#include "cartographer/common/mutex.h"
namespace cartographer {
namespace common {
/*
ThreadPool 是对c++11 thread的封装.
ThreadPool是线程数量固定的线程池，不可拷贝 和复制.
1，构造函数ThreadPool(int num_threads) 初始化一个线程数量固定的线程池。
2，Schedule(std::function<void()> work_item)添加想要ThreadPool执行的函数,
std::thread会在线程后台依次排队执行相关函数.
3，数据成员pool_是具体的线程，work_queue_是待执行的函数队列。
*/
// A fixed number of threads working on a work queue of work items. Adding a
// new work item does not block, and will be executed by a background thread
// eventually. The queue must be empty before calling the destructor. The thread
// pool will then wait for the currently executing work items to finish and then
// destroy the threads.
class ThreadPool {
public:
explicit ThreadPool(int num_threads);
~ThreadPool();
ThreadPool(const ThreadPool&) = delete;
ThreadPool& operator=(const ThreadPool&) = delete;
void Schedule(std::function<void()> work_item);
private:
void DoWork();
Mutex mutex_; //互斥锁，保证线程安全
//running_只是一个监视哨,只有线程池在running_状态时,才能往work_queue_加入函数.
bool running_ GUARDED_BY(mutex_) = true;
std::vector<std::thread> pool_ GUARDED_BY(mutex_);
std::deque<std::function<void()>> work_queue_ GUARDED_BY(mutex_);
};
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_
================================================
FILE: 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
================================================
FILE: 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*/
================================================
FILE: ground_truth/compute_relations_metrics_main.cc
================================================
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
DEFINE_string(
trajectory_filename, "",
"Proto containing the trajectory of which to assess the quality.");
DEFINE_string(relations_filename, "",
"Relations file containing the ground truth.");
namespace cartographer {
namespace ground_truth {
namespace {
transform::Rigid3d LookupPose(const transform::TransformInterpolationBuffer&
transform_interpolation_buffer,
const double time) {
constexpr int64 kUtsTicksPerSecond = 10000000;
common::Time common_time =
common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds *
kUtsTicksPerSecond) +
common::FromSeconds(time);
return transform_interpolation_buffer.Lookup(common_time);
}
struct Error {
double translational_squared;
double rotational_squared;
};
Error ComputeError(const transform::Rigid3d& pose1,
const transform::Rigid3d& pose2,
const transform::Rigid3d& expected) {
const transform::Rigid3d error =
(pose1.inverse() * pose2) * expected.inverse();
return Error{error.translation().squaredNorm(),
common::Pow2(transform::GetAngle(error))};
}
string MeanAndStdDevString(const std::vector<double>& values) {
CHECK_GE(values.size(), 2);
const double mean =
std::accumulate(values.begin(), values.end(), 0.) / values.size();
double sum_of_squared_differences = 0.;
for (const double value : values) {
sum_of_squared_differences += common::Pow2(value - mean);
}
const double standard_deviation =
std::sqrt(sum_of_squared_differences / (values.size() - 1));
std::ostringstream out;
out << std::fixed << std::setprecision(5) << mean << " +/- "
<< standard_deviation;
return string(out.str());
}
string StatisticsString(const std::vector<Error>& errors) {
std::vector<double> translational_errors;
std::vector<double> squared_translational_errors;
std::vector<double> rotational_errors_degrees;
std::vector<double> squared_rotational_errors_degrees;
for (const Error& error : errors) {
translational_errors.push_back(std::sqrt(error.translational_squared));
squared_translational_errors.push_back(error.translational_squared);
rotational_errors_degrees.push_back(
common::RadToDeg(std::sqrt(error.rotational_squared)));
squared_rotational_errors_degrees.push_back(
common::Pow2(rotational_errors_degrees.back()));
}
return "Abs translational error " +
MeanAndStdDevString(translational_errors) +
" m\n"
"Sqr translational error " +
MeanAndStdDevString(squared_translational_errors) +
" m^2\n"
"Abs rotational error " +
MeanAndStdDevString(rotational_errors_degrees) +
" deg\n"
"Sqr rotational error " +
MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
}
void Run(const string& trajectory_filename, const string& relations_filename) {
LOG(INFO) << "Reading trajectory from '" << trajectory_filename << "'...";
mapping::proto::Trajectory trajectory_proto;
{
std::ifstream trajectory_stream(trajectory_filename.c_str(),
std::ios::binary);
CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream));
}
const auto transform_interpolation_buffer =
transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto);
LOG(INFO) << "Reading relations from '" << relations_filename << "'...";
std::vector<Error> errors;
{
std::ifstream relations_stream(relations_filename.c_str());
double time1, time2, x, y, z, roll, pitch, yaw;
while (relations_stream >> time1 >> time2 >> x >> y >> z >> roll >> pitch >>
yaw) {
const auto pose1 = LookupPose(*transform_interpolation_buffer, time1);
const auto pose2 = LookupPose(*transform_interpolation_buffer, time2);
const transform::Rigid3d expected =
transform::Rigid3d(transform::Rigid3d::Vector(x, y, z),
transform::RollPitchYaw(roll, pitch, yaw));
errors.push_back(ComputeError(pose1, pose2, expected));
}
CHECK(relations_stream.eof());
}
LOG(INFO) << "Result:\n" << StatisticsString(errors);
}
} // namespace
} // namespace ground_truth
} // namespace cartographer
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
FLAGS_logtostderr = true;
google::SetUsageMessage(
"\n\n"
"This program computes the relation based metric described in:\n"
"R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n"
"C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n"
"algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.\n"
"\n"
"Note: Timestamps in the relations_file are interpreted relative to\n"
" the Unix epoch.");
google::ParseCommandLineFlags(&argc, &argv, true);
if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) {
google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics");
return EXIT_FAILURE;
}
::cartographer::ground_truth::Run(FLAGS_trajectory_filename,
FLAGS_relations_filename);
}
================================================
FILE: io/cairo_types.h
================================================
#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_
#define CARTOGRAPHER_IO_CAIRO_TYPES_H_
#include <memory>
#include "cairo/cairo.h"
/*
Cairo是一个2D图形库，支持多种输出设备。
cairo 是一个免费的矢量绘图软件库，它可以绘制多种输出格式。
https://www.ibm.com/developerworks/cn/linux/l-cairo/
http://liyanrui.is-programmer.com/2009/3/18/cairo-tutorial-05.7742.html
http://blog.csdn.net/turingo/article/details/8131057
*/
namespace cartographer {
namespace io {
namespace cairo {
//智能指针管理对象生命期.在unique_ptr销毁时释放指向的对象资源.
//对象类型是cairo_surface_t,自定义删除器deleter是函数指针.
// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniqueSurfacePtr =
std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;
//对象类型是cairo_t,自定义删除器deleter是函数指针.
// std::unique_ptr for Cairo contexts. The context is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniqueContextPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;
//对象类型是cairo_path_t,自定义删除器deleter是函数指针.
// std::unique_ptr for Cairo paths. The path is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniquePathPtr = std::unique_ptr<cairo_path_t, void (*)(cairo_path_t*)>;
} // namespace cairo
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_
================================================
FILE: io/coloring_points_processor.cc
================================================
#include "cartographer/io/coloring_points_processor.h"
#include "Eigen/Core"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
/*
根据assets_writer_backpack_2d.lua配置:.lua文件概览:
-- Now we recolor our points by frame and write another batch of X-Rays. It
-- is visible in them what was seen by the horizontal and the vertical
-- laser.
{
action = "color_points",
frame_id = "horizontal_laser_link",
color = { 255., 0., 0. },
},
{
action = "color_points",
frame_id = "vertical_laser_link",
color = { 0., 255., 0. },
},
FromDictionary()根据.lua配置文件获取frame_id下color的{r,g,b}
*/
std::unique_ptr<ColoringPointsProcessor>
ColoringPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
/*
frame_id:horizontal_laser_link或者vertical_laser_link
color:{ 255., 0., 0. },或者{ 0., 255., 0. },
*/
const string frame_id = dictionary->GetString("frame_id");
const std::vector<double> color_values =
dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
const Color color = {{static_cast<uint8_t>(color_values[0]),
static_cast<uint8_t>(color_values[1]),
static_cast<uint8_t>(color_values[2])}};
return common::make_unique<ColoringPointsProcessor>(color, frame_id, next);
}
ColoringPointsProcessor::ColoringPointsProcessor(const Color& color,
const string& frame_id,
PointsProcessor* const next)
: color_(color), frame_id_(frame_id), next_(next) {}
/*
只对相同的frame_id_处理：着色
*/
void ColoringPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
if (batch->frame_id == frame_id_) {
batch->colors.clear();
for (size_t i = 0; i < batch->points.size(); ++i) {
batch->colors.push_back(color_);//用color 填充batch的colors
}
}
next_->Process(std::move(batch));
}
PointsProcessor::FlushResult ColoringPointsProcessor::Flush() {
return next_->Flush();
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/coloring_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
{
action = "color_points",
frame_id = "horizontal_laser_link",
color = { 255., 0., 0. },
},
{
action = "color_points",
frame_id = "vertical_laser_link",
color = { 0., 255., 0. },
},
ColoringPointsProcessor是PointsProcessor的第六子类(6).
功能: 用固定的Color填充PointsBatch的Color分量。
数据成员:
1),color_：rgb值
2),frame_id_:只有相同的id才填充Color，color一般为[255,0,0],[0,255,0]
2),next_下一阶段的PointsProcessor.
*/
// Colors points with a fixed color by frame_id.
class ColoringPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "color_points";
ColoringPointsProcessor(const Color& color, const string& frame_id,
PointsProcessor* next);
static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~ColoringPointsProcessor() override{};
ColoringPointsProcessor(const ColoringPointsProcessor&) = delete;
ColoringPointsProcessor& operator=(const ColoringPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
const Color color_;
const string frame_id_;
PointsProcessor* const next_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_
================================================
FILE: io/counting_points_processor.cc
================================================
#include "cartographer/io/counting_points_processor.h"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
//初始化是num是0
CountingPointsProcessor::CountingPointsProcessor(PointsProcessor* next)
: num_points_(0), next_(next) {}
//与其他FromDictionary()函数统一写法
std::unique_ptr<CountingPointsProcessor>
CountingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<CountingPointsProcessor>(next);
}
//不处理points,而是将num_points_加上batch.size(),，即统计点云数据。然后直接流水线到下一PointsProcessor
void CountingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
num_points_ += batch->points.size();//相加
next_->Process(std::move(batch));
}
//依据下一阶段的状态重置本阶段的状态。
PointsProcessor::FlushResult CountingPointsProcessor::Flush() {
switch (next_->Flush()) {
case FlushResult::kFinished:
LOG(INFO) << "Processed " << num_points_ << " and finishing.";
return FlushResult::kFinished;
case FlushResult::kRestartStream:
LOG(INFO) << "Processed " << num_points_ << " and restarting stream.";
num_points_ = 0; //重启,则置为0
return FlushResult::kRestartStream;
}
LOG(FATAL);
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/counting_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
PointsProcessor的第二个子类(2).
功能:记录有多少 point被输出处理
数据成员:
1),num_points_记录points数量
2),next_下一阶段的PointsProcessor.
*/
// Passes through points, but keeps track of how many points it saw and output
// that on Flush.
class CountingPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "dump_num_points";
explicit CountingPointsProcessor(PointsProcessor* next);
static std::unique_ptr<CountingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~CountingPointsProcessor() override {}
CountingPointsProcessor(const CountingPointsProcessor&) = delete;
CountingPointsProcessor& operator=(const CountingPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> points) override;
FlushResult Flush() override;
private:
int64 num_points_;
PointsProcessor* next_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_
================================================
FILE: io/file_writer.cc
================================================
#include "cartographer/io/file_writer.h"
namespace cartographer {
namespace io {
//out:File open for writing: the internal stream buffer supports output operations.
//binary:Operations are performed in binary mode rather than text.
StreamFileWriter::StreamFileWriter(const string& filename)
: out_(filename, std::ios::out | std::ios::binary) {}//打开,并以2进制方式写入
StreamFileWriter::~StreamFileWriter() {}
//写入len个char
bool StreamFileWriter::Write(const char* const data, const size_t len) {
if (out_.bad()) {
return false;
}
out_.write(data, len);
return !out_.bad();
}
bool StreamFileWriter::Close() { //关闭文件
if (out_.bad()) {
return false;
}
out_.close();
return !out_.bad();
}
bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) {
if (out_.bad()) {
return false;
}
out_.flush(); //清空buffer
out_.seekp(0); //偏移量为0
return Write(data, len);
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/file_writer.h
================================================
#ifndef CARTOGRAPHER_IO_FILE_WRITER_H_
#define CARTOGRAPHER_IO_FILE_WRITER_H_
#include <fstream>
#include <functional>
#include <memory>
#include "cartographer/common/port.h"
namespace cartographer {
namespace io {
/*
FileWriter负责文件写入的虚基类,不可拷贝/赋值
没有数据成员.只提供一系列抽象接口.
1),WriteHeader(),写入文件head
2),Write(),写入数据
3),Close(),关闭文件
*/
// Simple abstraction for a file.
class FileWriter {
public:
FileWriter() {}
FileWriter(const FileWriter&) = delete;
FileWriter& operator=(const FileWriter&) = delete;
virtual ~FileWriter() {}
// Write 'data' to the beginning of the file. This is required to overwrite
// fixed size headers which contain the number of points once we actually know
// how many points there are.
virtual bool WriteHeader(const char* data, size_t len) = 0;
virtual bool Write(const char* data, size_t len) = 0;
virtual bool Close() = 0;
};
/*
StreamFileWriter 文件流写入类,继承自FileWriter
数据成员只有一个std::ofstream out_负责将文件写入磁盘
*/
// An Implementation of file using std::ofstream.
class StreamFileWriter : public FileWriter {
public:
~StreamFileWriter() override;
StreamFileWriter(const string& filename);
bool Write(const char* data, size_t len) override;
bool WriteHeader(const char* data, size_t len) override; //从文件开始处写入,覆盖旧数据
bool Close() override;
private:
std::ofstream out_;
};
/*工厂模式,
创建一个FileWriter对象,由智能指针管理生命期,
返回值是std::unique_ptr<FileWriter>,
函数参数是string.
*/
using FileWriterFactory =
std::function<std::unique_ptr<FileWriter>(const string& filename)>;
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_FILE_WRITER_H_
================================================
FILE: io/fixed_ratio_sampling_points_processor.cc
================================================
#include "cartographer/io/fixed_ratio_sampling_points_processor.h"
#include "Eigen/Core"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
/*
静态成员函数,利用参数构造一个智能指针指向FixedRatioSamplingPointsProcessor的对象
*/
std::unique_ptr<FixedRatioSamplingPointsProcessor>
FixedRatioSamplingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
const double sampling_ratio(dictionary->GetDouble("sampling_ratio"));
//sparse_pose_graph.lua:0.3,0.03
CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense.";
CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense.";
return common::make_unique<FixedRatioSamplingPointsProcessor>(sampling_ratio,
next);
}
/*
构造函数用采样率 sampling_ratio和PointsProcessor* next初始化
*/
FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor(
const double sampling_ratio, PointsProcessor* next)
: sampling_ratio_(sampling_ratio),
next_(next),
sampler_(new common::FixedRatioSampler(sampling_ratio_)) {}
/*
该类的核心函数,有几个为了提高效率的地方:
Process()传递智能指针,指向堆上的点集PointsBatch,
理论上该点集组成的vector是不断增加的.但是同时也有其他Process()在删除PointsBatch
*/
void FixedRatioSamplingPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
std::vector<int> to_remove; //保存索引
for (size_t i = 0; i < batch->points.size(); ++i) { //points是vector
if (!sampler_->Pulse()) {
//调用一次Pulse()其实影响到了sampler_的内部状态.
//模拟的传感器sampler_没有产生一个脉冲,则说明当前的batch[i]应该删除.
to_remove.push_back(i);
}
}
RemovePoints(to_remove, batch.get());
next_->Process(std::move(batch));//虚函数,调用子类的Process(),也就是这个函数本身
}
PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() {
switch (next_->Flush()) {
case PointsProcessor::FlushResult::kFinished:
//下一步已经完成,析构时则这一步也标记完成
return PointsProcessor::FlushResult::kFinished;
case PointsProcessor::FlushResult::kRestartStream://重启采样
//为何要重新赋值?:重启,不能带有以前的状态,所以重新初始化.
sampler_ =
common::make_unique<common::FixedRatioSampler>(sampling_ratio_);
return PointsProcessor::FlushResult::kRestartStream;
}
LOG(FATAL);
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/fixed_ratio_sampling_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
FixedRatioSamplingPointsProcessor是PointsProcessor的第七个子类(7).
FixedRatioSamplingPointsProcessor是采样过滤器,
该class只让具有固定采样频率的点通过,其余全部 remove.
sampling_ratio=1,即无任何操作,全通滤波.
sparse_pose_graph.lua:
sampling_ratio = 0.3,
*/
// Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1.
// makes this filter a no-op.
class FixedRatioSamplingPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"fixed_ratio_sampler";
FixedRatioSamplingPointsProcessor(double sampling_ratio,
PointsProcessor* next);
static std::unique_ptr<FixedRatioSamplingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~FixedRatioSamplingPointsProcessor() override{};
FixedRatioSamplingPointsProcessor(const FixedRatioSamplingPointsProcessor&) =
delete;
FixedRatioSamplingPointsProcessor& operator=(
const FixedRatioSamplingPointsProcessor&) = delete;
//根据采样率采集点云，不在采样点上的全部删除。
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
const double sampling_ratio_;//采样率
PointsProcessor* const next_;
std::unique_ptr<common::FixedRatioSampler> sampler_;//具有固定采样率的采样函数
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_
================================================
FILE: io/intensity_to_color_points_processor.cc
================================================
#include "cartographer/io/intensity_to_color_points_processor.h"
#include "Eigen/Core"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
std::unique_ptr<IntensityToColorPointsProcessor>
IntensityToColorPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
const string frame_id =
dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : "";
const float min_intensity = dictionary->GetDouble("min_intensity");
const float max_intensity = dictionary->GetDouble("max_intensity");
return common::make_unique<IntensityToColorPointsProcessor>(
min_intensity, max_intensity, frame_id, next);
}
IntensityToColorPointsProcessor::IntensityToColorPointsProcessor(
const float min_intensity, const float max_intensity,
const string& frame_id, PointsProcessor* const next)
: min_intensity_(min_intensity),
max_intensity_(max_intensity),
frame_id_(frame_id),
next_(next) {}
void IntensityToColorPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
if (!batch->intensities.empty() &&
(frame_id_.empty() || batch->frame_id == frame_id_)) {
batch->colors.clear();
for (const float intensity : batch->intensities) {
const uint8_t gray =
cartographer::common::Clamp(
(intensity - min_intensity_) / (max_intensity_ - min_intensity_),
0.f, 1.f) *
255;
batch->colors.push_back(Color{{gray, gray, gray}});
}
}
next_->Process(std::move(batch));
}
PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() {
return next_->Flush();
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/intensity_to_color_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
{
action = "intensity_to_color",
min_intensity = 0.,
max_intensity = 4095.,
},
PointsProcessor的第四个子类(4).
功能:将光强度转换为color
Intensity,图像亮度
IntensityToColorPointsProcessor：处理强度到color point 的强度变换类
成员函数执行转换:('intensity' - min ) / (max - min) * 255
*/
class IntensityToColorPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"intensity_to_color";
// Applies ('intensity' - min ) / (max - min) * 255 and color the point grey
// with this value for each point that comes from the sensor with 'frame_id'.
// If 'frame_id' is empty, this applies to all points.
IntensityToColorPointsProcessor(float min_intensity, float max_intensity,
const string& frame_id,
PointsProcessor* next);
static std::unique_ptr<IntensityToColorPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~IntensityToColorPointsProcessor() override{};
IntensityToColorPointsProcessor(const IntensityToColorPointsProcessor&) =
delete;
IntensityToColorPointsProcessor& operator=(
const IntensityToColorPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
const float min_intensity_;
const float max_intensity_;
const string frame_id_;
PointsProcessor* const next_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_
================================================
FILE: io/min_max_range_filtering_points_processor.cc
================================================
#include "cartographer/io/min_max_range_filtering_points_processor.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
//从.lua文件加载min_range和max_range,如:0-30
std::unique_ptr<MinMaxRangeFiteringPointsProcessor>
MinMaxRangeFiteringPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<MinMaxRangeFiteringPointsProcessor>(
dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), //trajectory_builder_2d.lua:0,30
//assets_writer_backpack_2d.lua 1-60
next);
}
/*
构造函数,传递min ,max2个范围参数和 PointsProcessor* next指针
*/
MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
const double min_range, const double max_range, PointsProcessor* next)
: min_range_(min_range), max_range_(max_range), next_(next) {}
void MinMaxRangeFiteringPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch)
{
std::vector<int> to_remove; //待移除的索引
for (size_t i = 0; i < batch->points.size(); ++i) {
//计算sensor到远点的l2距离
const float range = (batch->points[i] - batch->origin).norm();
if (!(min_range_ <= range && range <= max_range_)) {
//不在给定范围内,加入移除队列.
to_remove.push_back(i);
}
}
RemovePoints(to_remove, batch.get());
next_->Process(std::move(batch));//完成本轮过滤,接下来进行下一次过滤.即模拟流水线操作.
}
PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() {
return next_->Flush();
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/min_max_range_filtering_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
PointsProcessor的第一个子类(1).
功能:继承自PointsProcessor点云虚基类.距离过滤器,L2 距离不在Min-Max范围内的都过滤掉.
数据成员:
1),min_range_ 最小范围
2),max_range_ 最大范围
3),PointsProcessor* const next_:完成本轮Processor,接下来进行下一次Processor.
*/
// Filters all points that are farther away from their 'origin' as 'max_range'
// or closer than 'min_range'.
class MinMaxRangeFiteringPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"min_max_range_filter";
MinMaxRangeFiteringPointsProcessor(double min_range, double max_range,
PointsProcessor* next);
//从.lua文件加载min_range和max_range,如:0-30
static std::unique_ptr<MinMaxRangeFiteringPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~MinMaxRangeFiteringPointsProcessor() override {}//覆盖
MinMaxRangeFiteringPointsProcessor(
const MinMaxRangeFiteringPointsProcessor&) = delete;
MinMaxRangeFiteringPointsProcessor& operator=(
const MinMaxRangeFiteringPointsProcessor&) = delete;
//点云处理,删除[min,max]以外的point，并把数据传递到下一轮Processor处理。
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
//调用next_->Flush();父类调用Flush(),但在运行时才会决定是否调用子类的Flush()
private:
const double min_range_;
const double max_range_;
PointsProcessor* const next_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
================================================
FILE: io/null_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
NullPointsProcessor是PointsProcessor的第十个子类(10)
空操作类。通常用于pipline的尾端，丢弃所有的点云，表示整个处理流程已经完毕。
*/
// A points processor that just drops all points. The end of a pipeline usually.
class NullPointsProcessor : public PointsProcessor {
public:
NullPointsProcessor() {}
~NullPointsProcessor() override {}
//不作任何事情
void Process(std::unique_ptr<PointsBatch> points_batch) override {}
//返回"kFinished"状态，此操作会传递给上一个流水线。
FlushResult Flush() override { return FlushResult::kFinished; }
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_
================================================
FILE: io/outlier_removing_points_processor.cc
================================================
#include "cartographer/io/outlier_removing_points_processor.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
/*
VOXEL_SIZE = 5e-2
*/
std::unique_ptr<OutlierRemovingPointsProcessor>
OutlierRemovingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<OutlierRemovingPointsProcessor>(
dictionary->GetDouble("voxel_size"), next); //构造一个对象,返回一个智能指针
}
/*
构造函数,传递一个 voxel_size 和 PointsProcessor* next
*/
OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
const double voxel_size, PointsProcessor* next)
: voxel_size_(voxel_size),
next_(next),
state_(State::kPhase1),
voxels_(voxel_size_) {
LOG(INFO) << "Marking hits...";
}
/*
根据3个不同的state分别处理 points
*/
void OutlierRemovingPointsProcessor::Process(
std::unique_ptr<PointsBatch> points) {
switch (state_) {
case State::kPhase1:
ProcessInPhaseOne(*points);
break;
case State::kPhase2:
ProcessInPhaseTwo(*points);
break;
case State::kPhase3:
ProcessInPhaseThree(std::move(points));
break;
}
}
/*
更新state,并返回 FlushResult结果
*/
PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() {
switch (state_) {
case State::kPhase1:
LOG(INFO) << "Counting rays...";
state_ = State::kPhase2;
return FlushResult::kRestartStream;
case State::kPhase2:
LOG(INFO) << "Filtering outliers...";
state_ = State::kPhase3;
return FlushResult::kRestartStream;
case State::kPhase3:
CHECK(next_->Flush() == FlushResult::kFinished)
<< "Voxel filtering and outlier removal must be configured to occur "
"after any stages that require multiple passes.";
// multiple passes：多次传输。
return FlushResult::kFinished;
}
LOG(FATAL);
}
/*
state 1，统计光线的数量。
*/
void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
const PointsBatch& batch) {
for (size_t i = 0; i < batch.points.size(); ++i) {
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
}
}
/*
*/
void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
const PointsBatch& batch) {
// TODO(whess): This samples every 'voxel_size' distance and could be improved
// by better ray casting, and also by marking the hits of the current range
// data to be excluded.
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f delta = batch.points[i] - batch.origin;
const float length = delta.norm();
for (float x = 0; x < length; x += voxel_size_) {
const auto index =
voxels_.GetCellIndex(batch.origin + (x / length) * delta);
if (voxels_.value(index).hits > 0) {
++voxels_.mutable_value(index)->rays;
}
}
}
}
void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
std::unique_ptr<PointsBatch> batch) {
constexpr double kMissPerHitLimit = 3;
std::vector<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i]));
if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
to_remove.push_back(i);
}
}
RemovePoints(to_remove, batch.get());
next_->Process(std::move(batch));
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/outlier_removing_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
namespace cartographer {
namespace io {
/*
OutlierRemovingPointsProcessor是PointsProcessor的第八个子类(8).
OutlierRemovingPointsProcessor：
作用：体素过滤器,Remove有移动痕迹的体素。只保留没有移动的体素
数据成员：
const double voxel_size_;
PointsProcessor* const next_;
State state_;
mapping_3d::HybridGridBase<VoxelData> voxels_;
*/
// Voxel filters the data and only passes on points that we believe are on
// non-moving objects.
class OutlierRemovingPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"voxel_filter_and_remove_moving_objects";
OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next);
static std::unique_ptr<OutlierRemovingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~OutlierRemovingPointsProcessor() override {}
OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) =
delete;
OutlierRemovingPointsProcessor& operator=(
const OutlierRemovingPointsProcessor&) = delete;
//删除移动的体素。
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
// To reduce memory consumption by not having to keep all rays in memory, we
// filter outliers in three phases each going over all data: First we compute
// all voxels containing any hits, then we compute the rays passing through
// each of these voxels, and finally we output all hits in voxels that are
// considered obstructed.
struct VoxelData {
int hits = 0;
int rays = 0;
};
enum class State {
kPhase1,
kPhase2,
kPhase3,
};
// First phase counts the number of hits per voxel.
void ProcessInPhaseOne(const PointsBatch& batch);
// Second phase counts how many rays pass through each voxel. This is only
// done for voxels that contain hits. This is to reduce memory consumption by
// not adding data to free voxels.
void ProcessInPhaseTwo(const PointsBatch& batch);
// Third phase produces the output containing all inliers. We consider each
// hit an inlier if it is inside a voxel that has a sufficiently high
// hit-to-ray ratio.
void ProcessInPhaseThree(std::unique_ptr<PointsBatch> batch);
const double voxel_size_; //体素大小
PointsProcessor* const next_;
State state_;
mapping_3d::HybridGridBase<VoxelData> voxels_; //包含多个体素的网格Grid。
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_
================================================
FILE: io/pcd_writing_points_processor.cc
================================================
#include "cartographer/io/pcd_writing_points_processor.h"
#include <iomanip>
#include <sstream>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/points_batch.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
namespace {
/*
PCD文件由文件头和数据域组成，文件头存储了点云数据的字段格式信息，如：
========================================
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z rgba
SIZE 4 4 4 4
TYPE F F F U
COUNT 1 1 1 1
WIDTH 640
HEIGHT 480
VIEWPOINT 0 0 0 0 1 0 0
POINTS 307200
DATA binary
//下面是点云数据段
123.1 21.1 64.0
...
========================================
*/
// Writes the PCD header claiming 'num_points' will follow it into
// 'output_file'.
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
FileWriter* const file_writer) {
string color_header_field = !has_color ? "" : " rgb";
string color_header_type = !has_color ? "" : " U";
string color_header_size = !has_color ? "" : " 4";
string color_header_count = !has_color ? "" : " 1";
std::ostringstream stream;
stream << "# generated by Cartographer\n"
<< "VERSION .7\n"
<< "FIELDS x y z" << color_header_field << "\n"
<< "SIZE 4 4 4" << color_header_size << "\n"
<< "TYPE F F F" << color_header_type << "\n"
<< "COUNT 1 1 1" << color_header_count << "\n"
<< "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n"
<< "HEIGHT 1\n"
<< "VIEWPOINT 0 0 0 1 0 0 0\n"
<< "POINTS " << std::setw(15) << std::setfill('0') << num_points
<< "\n"
<< "DATA binary\n";
const string out = stream.str();
file_writer->WriteHeader(out.data(), out.size());
}
//{x,y,z}写入文件
void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
FileWriter* const file_writer) {
char buffer[12];
memcpy(buffer, &point[0], sizeof(float));
memcpy(buffer + 4, &point[1], sizeof(float)); //64机,float:4位
memcpy(buffer + 8, &point[2], sizeof(float));
CHECK(file_writer->Write(buffer, 12));
}
void WriteBinaryPcdPointColor(const Color& color,
FileWriter* const file_writer) {
char buffer[4];
buffer[0] = color[2];
buffer[1] = color[1];
buffer[2] = color[0];
buffer[3] = 0;
CHECK(file_writer->Write(buffer, 4));
}
} // namespace
std::unique_ptr<PcdWritingPointsProcessor>
PcdWritingPointsProcessor::FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next)
{
return common::make_unique<PcdWritingPointsProcessor>(
file_writer_factory(dictionary->GetString("filename")), next);
}
PcdWritingPointsProcessor::PcdWritingPointsProcessor(
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
: next_(next),
num_points_(0),
has_colors_(false),
file_writer_(std::move(file_writer)) {}
PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush()
{
WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
//调用Flush()才写入head信息:此时才知道num_points_的值
CHECK(file_writer_->Close());
switch (next_->Flush()) {
case FlushResult::kFinished:
return FlushResult::kFinished;
case FlushResult::kRestartStream://不能多次传递同一数据.
LOG(FATAL) << "PCD generation must be configured to occur after any "
"stages that require multiple passes.";
}
LOG(FATAL);
}
/*
核心函数,按照{x,y,z}写入文件
*/
void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch)
{
if (batch->points.empty()) {
next_->Process(std::move(batch));
return;
}
if (num_points_ == 0) {
has_colors_ = !batch->colors.empty();
WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
}
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
if (!batch->colors.empty()) {
WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get());
}
++num_points_;//统计处理了多少points
}
next_->Process(std::move(batch));
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/pcd_writing_points_processor.h
================================================
#include <fstream>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
/*
点云是目标表面特性的海量点集合。
根据激光测量原理得到的点云，包括三维坐标（XYZ）和激光反射强度（Intensity）。
根据摄影测量原理得到的点云，包括三维坐标（XYZ）和颜色信息（RGB）。
结合激光测量和摄影测量原理得到点云，包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）。
在获取物体表面每个采样点的空间坐标后，得到的是一个点的集合，称之为“点云”(Point Cloud)。
pcd文件存储了每个点的x,y,z坐标以及r,g,b,a颜色信息
*/
namespace cartographer {
namespace io {
/*
PcdWritingPointsProcessor是PointsProcessor的第九个子类(9)
.
PcdWritingPointsProcessor类将点云按照pcd格式存储在pcd文件中.
1,构造函数初始化一个文件类名,
2,成员函数FromDictionary 从配置文件读取 文件名
3,Process()和Flush()负责写入文件
4,数据成员next_指向 下一阶段的PointsProcessor点云处理类
*/
// Streams a PCD file to disk. The header is written in 'Flush'.
class PcdWritingPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "write_pcd";
PcdWritingPointsProcessor(std::unique_ptr<FileWriter> file_writer,
PointsProcessor* next);
static std::unique_ptr<PcdWritingPointsProcessor> FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~PcdWritingPointsProcessor() override {}
PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete;
PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) =
delete;
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
PointsProcessor* const next_;
int64 num_points_;
bool has_colors_;
std::unique_ptr<FileWriter> file_writer_;
};
} // namespace io
} // namespace cartographer
================================================
FILE: io/ply_writing_points_processor.cc
================================================
#include "cartographer/io/ply_writing_points_processor.h"
#include <iomanip>
#include <sstream>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/points_batch.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
namespace {
/*
按照ply格式写入ply的head信息.此函数在最后写入.
why?num_points_只有最后才知道.之前尚未统计完成.
*/
// Writes the PLY header claiming 'num_points' will follow it into
// 'output_file'.
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
FileWriter* const file_writer) {
string color_header = !has_color ? ""
: "property uchar red\n"
"property uchar green\n"
"property uchar blue\n";
std::ostringstream stream;
stream << "ply\n"
<< "format binary_little_endian 1.0\n"
<< "comment generated by Cartographer\n"
<< "element vertex " << std::setw(15) << std::setfill('0')
<< num_points << "\n"
<< "property float x\n"
<< "property float y\n"
<< "property float z\n"
<< color_header << "end_header\n";
const string out = stream.str();
CHECK(file_writer->WriteHeader(out.data(), out.size()));
}
/*
往file_writer写入某一point对应的{x,y,z}
*/
void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
FileWriter* const file_writer) {
char buffer[12];
memcpy(buffer, &point[0], sizeof(float));
memcpy(buffer + 4, &point[1], sizeof(float));
memcpy(buffer + 8, &point[2], sizeof(float));
CHECK(file_writer->Write(buffer, 12));
}
/*
往file_writer写入rgb值
*/
void WriteBinaryPlyPointColor(const Color& color,
FileWriter* const file_writer) {
CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
color.size()));
}
} // namespace
/*
根据.lua文件的filename创建file_writer_factory
*/
std::unique_ptr<PlyWritingPointsProcessor>
PlyWritingPointsProcessor::FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<PlyWritingPointsProcessor>(
file_writer_factory(dictionary->GetString("filename")), next);
}
/*
构造函数,默认点云数是0,无rgb值
*/
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
: next_(next),
num_points_(0),
has_colors_(false),
file_(std::move(file_writer)) {}
/*
在最后阶段才写入head,但不是在文件最后写入head,而是out_.seekp(0); 偏移到0处
*/
PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
WriteBinaryPlyHeader(has_colors_, num_points_, file_.get());
CHECK(file_->Close()) << "Closing PLY file_writer failed.";
switch (next_->Flush()) {
case FlushResult::kFinished:
return FlushResult::kFinished;
case FlushResult::kRestartStream:
LOG(FATAL) << "PLY generation must be configured to occur after any "
"stages that require multiple passes.";
}
LOG(FATAL);
}
/*
真正的写入点云PointsBatch,整个类的核心在这个函数.
*/
void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
if (batch->points.empty()) { //点云为空,则不写入磁盘,直接进行下一Process环节.
next_->Process(std::move(batch));
return;
}
if (num_points_ == 0) {//点云num为0则写入rgb,和head
has_colors_ = !batch->colors.empty();
WriteBinaryPlyHeader(has_colors_, 0, file_.get());
//file_.get()获取指针指针对应的raw指针
}
if (has_colors_) { //rgb的vector长度和point的vecor长度必须一致.
CHECK_EQ(batch->points.size(), batch->colors.size())
<< "First PointsBatch had colors, but encountered one without. "
"frame_id: "
<< batch->frame_id;
}
//函数核心,开始写入每一个point.
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
if (has_colors_) {
WriteBinaryPlyPointColor(batch->colors[i], file_.get());
}
++num_points_;
}
next_->Process(std::move(batch)); //继续流水线操作.
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/ply_writing_points_processor.h
================================================
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
PlyWritingPointsProcessor是PointsProcessor的第五个子类(5).
PlyWritingPointsProcessor负责将点云point以PLY的格式写入磁盘.
*/
// Streams a PLY file to disk. The header is written in 'Flush'.
class PlyWritingPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "write_ply";
// 指定文件写入的工厂和流水线next指针。
PlyWritingPointsProcessor(std::unique_ptr<FileWriter> file,
PointsProcessor* next);
static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~PlyWritingPointsProcessor() override {}
PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete;
PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) =
delete;
//按照ply格式写点云信息 。然后直接流水线到下一PointsProcessor
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
PointsProcessor* const next_;
int64 num_points_;
bool has_colors_;//是否是RGB
std::unique_ptr<FileWriter> file_;
};
} // namespace io
} // namespace cartographer
================================================
FILE: io/points_batch.cc
================================================
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
/*
先对to_remove做降序排列,然后按照to_remove依次移除某些point.
batch->points是vector,index 是要移除的vector元素的索引.
时间复杂度:o(m*n),m是to_remove的大小,n是to_remove[0]到batch的end()的距离.
注:erase()的复杂度是:
Linear on the number of elements erased (destructions)*
the number of elements after the last element deleted (moving).
返回值是一个迭代器，指向删除元素下一个元素;
使用erase()要注意迭代器失效的问题.
使用降序排序,即从end开始erase()可避免失效
*/
void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
std::sort(to_remove.begin(), to_remove.end(), std::greater<int>());//降序排列
for (const int index : to_remove) {
batch->points.erase(batch->points.begin() + index); //移除point
if (!batch->colors.empty()) {
batch->colors.erase(batch->colors.begin() + index); //point对应的rgb
}
if (!batch->intensities.empty()) { //移除光强度
batch->intensities.erase(batch->intensities.begin() + index);
}
}
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/points_batch.h
================================================
#ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_
#define CARTOGRAPHER_IO_POINTS_BATCH_H_
#include <array>
#include <cstdint>
#include <vector>
#include "Eigen/Core"
#include "cartographer/common/time.h"
namespace cartographer {
namespace io {
// A point's color.
using Color = std::array<uint8_t, 3>;
/*
PointsBatch类是对多个点云point的抽象.这些point在由“同一时刻”,“同一机器人坐标地点”的传感器采集而得。
数据成员主要描述了point的特性.
1,time:point采集时间.
2,origin:sensor的世界坐标
3,frame_id:帧id
4,trajectory_index:轨迹线id
5,points:几何参数,vector<{x,y,z}>
6,intensities:光强
7,colors:point的rgb值
*/
// A number of points, captured around the same 'time' and by a
// sensor at the same 'origin'.
struct PointsBatch {
PointsBatch() {
origin = Eigen::Vector3f::Zero();
trajectory_index = 0;
}
// Time at which this batch has been acquired. point被采集的时间点
common::Time time;
// Origin of the data, i.e. the location of the sensor in the world at
// 'time'. 传感器位姿
Eigen::Vector3f origin;
// Sensor that generated this data's 'frame_id' or empty if this information
// is unknown. 关键帧的id
string frame_id;
// Trajectory index that produced this point. 轨迹线
int trajectory_index;
// Geometry of the points in a metric frame. 公制，point的几何参数
std::vector<Eigen::Vector3f> points;
// Intensities are optional and may be unspecified. The meaning of these
// intensity values varies by device. For example, the VLP16 provides values
// in the range [0, 100] for non-specular return values and values up to 255
// for specular returns. On the other hand, Hokuyo lasers provide a 16-bit
// value that rarely peaks above 4096.
std::vector<float> intensities; //光强度，可选项
// Colors are optional. If set, they are RGB values.彩色rgb数字
std::vector<Color> colors;
};
// Removes the indices in 'to_remove' from 'batch'.
// 按照to_temove中的索引,在batch中移除某些point.
void RemovePoints(std::vector<int> to_remove, PointsBatch* batch);
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_POINTS_BATCH_H_
================================================
FILE: io/points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
/*
PointsProcessor 点云虚基类,提供一种批量处理points的抽象接口,不可拷贝/赋值
2个抽象接口:
1),Process()负责对PointsBatch进行处理
2),Flush()刷新.
在assets_writer_backpack_2d.lua文件中有各个pipeline的处理流程.
*/
// A processor in a pipeline. It processes a 'points_batch' and hands it to the
// next processor in the pipeline.
class PointsProcessor {
public:
enum class FlushResult {
kRestartStream,
kFinished,
};
PointsProcessor() {}
virtual ~PointsProcessor() {} //必须为虚函数,不然子类无法正确析构.
PointsProcessor(const PointsProcessor&) = delete;
PointsProcessor& operator=(const PointsProcessor&) = delete;
// Receive a 'points_batch', process it and pass it on.
virtual void Process(std::unique_ptr<PointsBatch> points_batch) = 0;//纯虚函数
// Some implementations will perform expensive computations and others that do
// multiple passes over the data might ask for restarting the stream.
virtual FlushResult Flush() = 0;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
================================================
FILE: io/points_processor_pipeline_builder.cc
================================================
#include "cartographer/io/points_processor_pipeline_builder.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/coloring_points_processor.h"
#include "cartographer/io/counting_points_processor.h"
#include "cartographer/io/fixed_ratio_sampling_points_processor.h"
#include "cartographer/io/intensity_to_color_points_processor.h"
#include "cartographer/io/min_max_range_filtering_points_processor.h"
#include "cartographer/io/null_points_processor.h"
#include "cartographer/io/outlier_removing_points_processor.h"
#include "cartographer/io/pcd_writing_points_processor.h"
#include "cartographer/io/ply_writing_points_processor.h"
#include "cartographer/io/xray_points_processor.h"
#include "cartographer/io/xyz_writing_points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
namespace cartographer {
namespace io {
/*
注册内置的points Processor处理类,
只有1个参数，类型是PointsProcessorPipelineBuilder。
具体做法是调用Builder的成员函数
Register(const std::string& name, FactoryFunction factory);
第一参数是kConfigurationFileActionName
第二参数是FactoryFunction,即为函数unique_ptr<> f(dictionary,next),
返回值是调用每个Processor的成员函数FromDictionary()
得到unique_ptr<PointsProcessor>.
*/
template <typename PointsProcessorType>
void RegisterPlainPointsProcessor(
PointsProcessorPipelineBuilder* const builder)
{
builder->Register(
PointsProcessorType::kConfigurationFileActionName,
[](common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next)
-> std::unique_ptr<PointsProcessor>//返回值是unique_ptr
{
return PointsProcessorType::FromDictionary(dictionary, next);
});
}
/*
RegisterFileWritingPointsProcessor负责注册文件写入的Processor类。
用于将points写入到文件。
第一参数：管理文件写入的FileWriterFactory
第二参数：PointsProcessorPipelineBuilder*
*/
template <typename PointsProcessorType>
void RegisterFileWritingPointsProcessor(
FileWriterFactory file_writer_factory,
PointsProcessorPipelineBuilder* const builder)
{
builder->Register(
PointsProcessorType::kConfigurationFileActionName,
[file_writer_factory](
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next)
-> std::unique_ptr<PointsProcessor> //返回值是unique_ptr
{
return PointsProcessorType::FromDictionary(file_writer_factory,
dictionary, next);
});
}
/*.h文件对外的接口,主要的代码逻辑
作用：依次注册所有的内置Processor类。
*/
void RegisterBuiltInPointsProcessors(
const mapping::proto::Trajectory& trajectory,
FileWriterFactory file_writer_factory,
PointsProcessorPipelineBuilder* builder)
{
//注册多个非文件相关的Processor实例对象
RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
RegisterPlainPointsProcessor<ColoringPointsProcessor>(builder);
RegisterPlainPointsProcessor<IntensityToColorPointsProcessor>(builder);
//注册多个file Processor实例对象
RegisterFileWritingPointsProcessor<PcdWritingPointsProcessor>(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<PlyWritingPointsProcessor>(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<XyzWriterPointsProcessor>(
file_writer_factory, builder);
//特殊：轨迹线传引用&id，用于区别不同建图楼层。
// X-Ray is an odd ball(古怪的，特殊的) since it requires the trajectory to figure out the
// different building levels we walked on to separate the images.
builder->Register(
XRayPointsProcessor::kConfigurationFileActionName,
[&trajectory, file_writer_factory]
(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next
)
-> std::unique_ptr<PointsProcessor>
{
return XRayPointsProcessor::FromDictionary(
trajectory, file_writer_factory, dictionary, next);
});
}
//将用name标识的class添加到hash表中
void PointsProcessorPipelineBuilder::Register(const std::string& name,
FactoryFunction factory) {
CHECK(factories_.count(name) == 0) << "A points processor named '" << name
<< "' has already been registered.";
factories_[name] = factory;
}
PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {}
/*
1）整个sensor文件夹下最核心的代码。根据.lua文件
用于创建pipeline流水线，登记所有的PointsProcessor类。
返回值pipeline 是points 处理类的集合。内含所有的class集合。
2）pipeline是vector，元素类型是unique_ptr,指向处理point的class。
3）第一个是空指针NullPointsProcessor类,最后调用,用于丢弃所有的points,
其余的按照逆序添加到pipeline中
*/
std::vector<std::unique_ptr<PointsProcessor>>
PointsProcessorPipelineBuilder::CreatePipeline(
common::LuaParameterDictionary* const dictionary) const {
std::vector<std::unique_ptr<PointsProcessor>> pipeline;
// The last consumer in the pipeline must exist, so that the one created after
// it (and being before it in the pipeline) has a valid 'next' to point to.
// The last consumer will just drop all points.
pipeline.emplace_back(common::make_unique<NullPointsProcessor>());
std::vector<std::unique_ptr<common::LuaParameterDictionary>> configurations =
dictionary->GetArrayValuesAsDictionaries();
//按action逆序，rbegin()，从尾部添加元素到pipeline中。
// We construct the pipeline starting at the back.
for (auto it = configurations.rbegin(); it != configurations.rend(); it++) {
const string action = (*it)->GetString("action");//min_max_range_filter
auto factory_it = factories_.find(action);
//类型是pair<,>，second是min_max对应的FactoryFunction
//只有在hash表中的action在可被添加：否则找不到函数实现。
CHECK(factory_it != factories_.end())
<< "Unknown action '" << action
<< "'. Did you register the correspoinding PointsProcessor?";
pipeline.push_back(factory_it->second(it->get(), pipeline.back().get()));
/*注意：
上述代码是vector.push_back( f(dict,next));
内部是一个函数，返回unique_ptr,然后插入vector.
vector.back()总是next的Processor。
*/
}
return pipeline;
}
} // namespace io
} // namespace cartographer
/*
pipeline = {
{
action = "min_max_range_filter",--1
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points", --2
},
-- Gray X-Rays. These only use geometry to color pixels.
{
action = "write_xray_image", --3
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all",
transform = YZ_TRANSFORM,
},
....
}
*/
================================================
FILE: io/points_processor_pipeline_builder.h
================================================
#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_
#define CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_
#include <string>
#include <unordered_map>
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
namespace cartographer {
namespace io {
/*
使用Builder模式创建PointsProcessor对象：
PointsProcessorPipelineBuilder类用于创建各个ProcessorPiont类对象
调用RegisterBuiltInPointsProcessors()
函数可用于创建所有cartographer内置的PointsProcessor类.
*/
// Builder to create a points processor pipeline out of a Lua configuration.
// You can register all built-in PointsProcessors using
// 'RegisterBuiltInPointsProcessors'. Non-built-in PointsProcessors must define
// a name and a factory method for building itself from a
// LuaParameterDictionary. See the various built-in PointsProcessors for
// examples.
class PointsProcessorPipelineBuilder {
public:
//FactoryFunction是函数f(dictionary,next);
//返回值为std::unique_ptr<PointsProcessor>
using FactoryFunction = std::function<std::unique_ptr<PointsProcessor> (
common::LuaParameterDictionary*, PointsProcessor* next) >;
PointsProcessorPipelineBuilder();
PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) =
delete;
PointsProcessorPipelineBuilder& operator=(
const PointsProcessorPipelineBuilder&) = delete;
// Register a new PointsProcessor type uniquly identified by 'name' which will
// be created using 'factory'.
void Register(const std::string& name, FactoryFunction factory);
std::vector<std::unique_ptr<PointsProcessor>> CreatePipeline(
common::LuaParameterDictionary* dictionary) const;
private:
//hash表,记录已有代码实现的points processor及其对应的function
//key是每个class都有的name。
std::unordered_map<std::string, FactoryFunction> factories_;
};
// Register all 'PointsProcessor' that ship with Cartographer with this
// 'builder'.
void RegisterBuiltInPointsProcessors(
const mapping::proto::Trajectory& trajectory,
FileWriterFactory file_writer_factory,
PointsProcessorPipelineBuilder* builder);
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_
================================================
FILE: io/proto_stream.cc
================================================
#include "cartographer/io/proto_stream.h"
namespace cartographer {
namespace io {
namespace {
// First eight bytes to identify our proto stream format.
const size_t kMagic = 0x7b1d1f7b5bf501db;//文件头标识符
void WriteSizeAsLittleEndian(size_t size, std::ostream* out) {
for (int i = 0; i != 8; ++i) {
out->put(size & 0xff);
size >>= 8;
}
}
bool ReadSizeAsLittleEndian(std::istream* in, size_t* size) {
*size = 0;
for (int i = 0; i != 8; ++i) {
*size >>= 8;
*size += static_cast<size_t>(in->get()) << 56;
}
return !in->fail();
}
} // namespace
ProtoStreamWriter::ProtoStreamWriter(const string& filename)
: out_(filename, std::ios::out | std::ios::binary) {
WriteSizeAsLittleEndian(kMagic, &out_);
}
ProtoStreamWriter::~ProtoStreamWriter() {}
void ProtoStreamWriter::Write(const string& uncompressed_data) {
string compressed_data;
common::FastGzipString(uncompressed_data, &compressed_data);
WriteSizeAsLittleEndian(compressed_data.size(), &out_);
out_.write(compressed_data.data(), compressed_data.size());
}
bool ProtoStreamWriter::Close() {
out_.close();
return !out_.fail();
}
ProtoStreamReader::ProtoStreamReader(const string& filename)
: in_(filename, std::ios::in | std::ios::binary) {
size_t magic;
if (!ReadSizeAsLittleEndian(&in_, &magic) || magic != kMagic) {
in_.setstate(std::ios::failbit);
}
}
ProtoStreamReader::~ProtoStreamReader() {}
bool ProtoStreamReader::Read(string* decompressed_data) {
size_t compressed_size;
if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) {
return false;
}
string compressed_data(compressed_size, '\0');
if (!in_.read(&compressed_data.front(), compressed_size)) {
return false;
}
common::FastGunzipString(compressed_data, decompressed_data);
return true;
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/proto_stream.h
================================================
#ifndef CARTOGRAPHER_IO_PROTO_STREAM_H_
#define CARTOGRAPHER_IO_PROTO_STREAM_H_
#include <fstream>
#include "cartographer/common/port.h"
namespace cartographer {
namespace io {
/*
ProtoStreamWriter类用于将压缩的序列化数据protocol messages 写入文件
ProtoStreamReader类用于读取上述已经compressed的文件
*/
// A simple writer of a compressed sequence of protocol buffer messages to a
// file. The format is not intended to be compatible with any other format used
// outside of Cartographer.
//
// TODO(whess): Compress the file instead of individual messages for better
// compression performance? Should we use LZ4?
class ProtoStreamWriter {
public:
ProtoStreamWriter(const string& filename);
~ProtoStreamWriter();
ProtoStreamWriter(const ProtoStreamWriter&) = delete;
ProtoStreamWriter& operator=(const ProtoStreamWriter&) = delete;
// Serializes, compressed and writes the 'proto' to the file.
template <typename MessageType>
void WriteProto(const MessageType& proto) {
string uncompressed_data;
proto.SerializeToString(&uncompressed_data);
Write(uncompressed_data); //Write()函数执行压缩过程。
}
// This should be called to check whether writing was successful.
bool Close();
private:
void Write(const string& uncompressed_data);
std::ofstream out_;
};
// A reader of the format produced by ProtoStreamWriter.
class ProtoStreamReader {
public:
ProtoStreamReader(const string& filename);
~ProtoStreamReader();
ProtoStreamReader(const ProtoStreamReader&) = delete;
ProtoStreamReader& operator=(const ProtoStreamReader&) = delete;
template <typename MessageType>
bool ReadProto(MessageType* proto) {
string decompressed_data;
return Read(&decompressed_data) &&
proto->ParseFromString(decompressed_data);
}
private:
bool Read(string* decompressed_data);
std::ifstream in_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_PROTO_STREAM_H_
================================================
FILE: io/proto_stream_test.cc
================================================
#include "cartographer/io/proto_stream.h"
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "cartographer/common/port.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace io {
namespace {
class ProtoStreamTest : public ::testing::Test {
protected:
void SetUp() override {
const string tmpdir = P_tmpdir;
test_directory_ = tmpdir + "/proto_stream_test_XXXXXX";
ASSERT_NE(mkdtemp(&test_directory_[0]), nullptr) << strerror(errno);
}
void TearDown() override { remove(test_directory_.c_str()); }
string test_directory_;
};
TEST_F(ProtoStreamTest, WriteAndReadBack) {
const string test_file = test_directory_ + "/test_trajectory.pbstream";
{
ProtoStreamWriter writer(test_file);
for (int i = 0; i != 10; ++i) {
mapping::proto::Trajectory trajectory;
trajectory.add_node()->set_timestamp(i);
writer.WriteProto(trajectory);
}
ASSERT_TRUE(writer.Close());
}
{
ProtoStreamReader reader(test_file);
for (int i = 0; i != 10; ++i) {
mapping::proto::Trajectory trajectory;
ASSERT_TRUE(reader.ReadProto(&trajectory));
ASSERT_EQ(1, trajectory.node_size());
EXPECT_EQ(i, trajectory.node(0).timestamp());
}
mapping::proto::Trajectory trajectory;
EXPECT_FALSE(reader.ReadProto(&trajectory));
}
remove(test_file.c_str());
}
} // namespace
} // namespace io
} // namespace cartographer
================================================
FILE: io/xray_points_processor.cc
================================================
#include "cartographer/io/xray_points_processor.h"
#include <cmath>
#include <string>
#include "Eigen/Core"
#include "cairo/cairo.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/io/cairo_types.h"
#include "cartographer/mapping/detect_floors.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
namespace cartographer {
namespace io {
namespace {
struct PixelData {
size_t num_occupied_cells_in_column = 0;
double mean_r = 0.;
double mean_g = 0.;
double mean_b = 0.;
};
using PixelDataMatrix =
Eigen::Matrix<PixelData, Eigen::Dynamic, Eigen::Dynamic>;
double Mix(const double a, const double b, const double t) {
return a * (1. - t) + t * b;
}
//cairo回调函数，将data写入FileWriter
cairo_status_t CairoWriteCallback(void* const closure,
const unsigned char* data,
const unsigned int length) {
if (static_cast<FileWriter*>(closure)->Write(
reinterpret_cast<const char*>(data), length)) {
return CAIRO_STATUS_SUCCESS;
}
return CAIRO_STATUS_WRITE_ERROR;
}
/*
使用cairo库将mat以png文件写入filename
*/
// Write 'mat' as a pleasing-to-look-at PNG into 'filename'
void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
const int stride =
cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
CHECK_EQ(stride % 4, 0);
std::vector<uint32_t> pixels(stride / 4 * mat.rows(), 0.);
float max = std::numeric_limits<float>::min();
for (int y = 0; y < mat.rows(); ++y) {
for (int x = 0; x < mat.cols(); ++x) {
const PixelData& cell = mat(y, x);
if (cell.num_occupied_cells_in_column == 0.) {
continue;
}
max = std::max<float>(max, std::log(cell.num_occupied_cells_in_column));
}
}
for (int y = 0; y < mat.rows(); ++y) {
for (int x = 0; x < mat.cols(); ++x) {
const PixelData& cell = mat(y, x);
if (cell.num_occupied_cells_in_column == 0.) {
pixels[y * stride / 4 + x] =
(255 << 24) | (255 << 16) | (255 << 8) | 255;
continue;
}
// We use a logarithmic weighting for how saturated a pixel will be. The
// basic idea here was that walls (full height) are fully saturated, but
// details like chairs and tables are still well visible.
const float saturation =
std::log(cell.num_occupied_cells_in_column) / max;
double mean_r_in_column = (cell.mean_r / 255.);
double mean_g_in_column = (cell.mean_g / 255.);
double mean_b_in_column = (cell.mean_b / 255.);
double mix_r = Mix(1., mean_r_in_column, saturation);
double mix_g = Mix(1., mean_g_in_column, saturation);
double mix_b = Mix(1., mean_b_in_column, saturation);
const int r = common::RoundToInt(mix_r * 255.);
const int g = common::RoundToInt(mix_g * 255.);
const int b = common::RoundToInt(mix_b * 255.);
pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b;
}
}
// TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
// the data until the surface is finalized. Once it is finalized though,
// cairo_surface_write_to_png fails, complaining that the surface is already
// finalized. This makes it pretty hard to pass back ownership of the image to
// the caller.
cairo::UniqueSurfacePtr surface(
cairo_image_surface_create_for_data(
reinterpret_cast<unsigned char*>(pixels.data()), CAIRO_FORMAT_ARGB32,
mat.cols(), mat.rows(), stride),
cairo_surface_destroy);
CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
file_writer),
CAIRO_STATUS_SUCCESS);
CHECK(file_writer->Close());
}
bool ContainedIn(const common::Time& time,
const std::vector<mapping::Timespan>& timespans) {
for (const mapping::Timespan& timespan : timespans) {
if (timespan.start <= time && time <= timespan.end) {
return true;
}
}
return false;
}
} // namespace
XRayPointsProcessor::XRayPointsProcessor(
const double voxel_size, const transform::Rigid3f& transform,
const std::vector<mapping::Floor>& floors, const string& output_filename,
FileWriterFactory file_writer_factory, PointsProcessor* const next)
: next_(next),
file_writer_factory_(file_writer_factory),
floors_(floors),
output_filename_(output_filename),
transform_(transform) {
for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
aggregations_.emplace_back(
Aggregation{mapping_3d::HybridGridBase<bool>(voxel_size), {}});
}
}
std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
const mapping::proto::Trajectory& trajectory,
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
std::vector<mapping::Floor> floors;
if (dictionary->HasKey("separate_floors") &&
dictionary->GetBool("separate_floors")) {
floors = mapping::DetectFloors(trajectory);
}
return common::make_unique<XRayPointsProcessor>(
dictionary->GetDouble("voxel_size"),
transform::FromDictionary(dictionary->GetDictionary("transform").get())
.cast<float>(),
floors, dictionary->GetString("filename"), file_writer_factory, next);
}
void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
FileWriter* const file_writer) {
if (bounding_box_.isEmpty()) {
LOG(WARNING) << "Not writing output: bounding box is empty.";
return;
}
// Returns the (x, y) pixel of the given 'index'.
const auto voxel_index_to_pixel = [this](const Eigen::Array3i& index) {
// We flip the y axis, since matrices rows are counted from the top.
return Eigen::Array2i(bounding_box_.max()[1] - index[1],
bounding_box_.max()[2] - index[2]);
};
// Hybrid grid uses X: forward, Y: left, Z: up.
// For the screen we are using. X: right, Y: up
const int xsize = bounding_box_.sizes()[1] + 1;
const int ysize = bounding_box_.sizes()[2] + 1;
PixelDataMatrix image = PixelDataMatrix(ysize, xsize);
for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels);
!it.Done(); it.Next()) {
const Eigen::Array3i cell_index = it.GetCellIndex();
const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index);
PixelData& pixel_data = image(pixel.y(), pixel.x());
const auto& column_data = aggregation.column_data.at(
std::make_pair(cell_index[1], cell_index[2]));
pixel_data.mean_r = column_data.sum_r / column_data.count;
pixel_data.mean_g = column_data.sum_g / column_data.count;
pixel_data.mean_b = column_data.sum_b / column_data.count;
++pixel_data.num_occupied_cells_in_column;
}
WritePng(image, file_writer);
}
void XRayPointsProcessor::Insert(const PointsBatch& batch,
const transform::Rigid3f& transform,
Aggregation* const aggregation) {
constexpr Color kDefaultColor = {{0, 0, 0}};
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f camera_point = transform * batch.points[i];
const Eigen::Array3i cell_index =
aggregation->voxels.GetCellIndex(camera_point);
*aggregation->voxels.mutable_value(cell_index) = true;
bounding_box_.extend(cell_index.matrix());
ColumnData& column_data =
aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])];
const auto& color =
batch.colors.empty() ? kDefaultColor : batch.colors.at(i);
column_data.sum_r += color[0];
column_data.sum_g += color[1];
column_data.sum_b += color[2];
++column_data.count;
}
}
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
if (floors_.empty()) {
CHECK_EQ(aggregations_.size(), 1);
Insert(*batch, transform_, &aggregations_[0]);
} else {
for (size_t i = 0; i < floors_.size(); ++i) {
if (!ContainedIn(batch->time, floors_[i].timespans)) {
continue;
}
Insert(*batch, transform_, &aggregations_[i]);
}
}
next_->Process(std::move(batch));
}
PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
if (floors_.empty()) {
CHECK_EQ(aggregations_.size(), 1);
WriteVoxels(aggregations_[0],
file_writer_factory_(output_filename_ + ".png").get());
} else {
for (size_t i = 0; i < floors_.size(); ++i) {
WriteVoxels(
aggregations_[i],
file_writer_factory_(output_filename_ + std::to_string(i) + ".png")
.get());
}
}
switch (next_->Flush()) {
case FlushResult::kRestartStream:
LOG(FATAL) << "X-Ray generation must be configured to occur after any "
"stages that require multiple passes.";
case FlushResult::kFinished:
return FlushResult::kFinished;
}
LOG(FATAL);
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/xray_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
#include <map>
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/mapping/detect_floors.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/transform/rigid_transform.h"
/*配置文件概览：
VOXEL_SIZE = 5e-2
YZ_TRANSFORM = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., math.pi, },
}
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz_all_intensity",
transform = YZ_TRANSFORM,
},
*/
namespace cartographer {
namespace io {
/*
xray_points_processor.h是PointsProcessor的第三个子类(3).
功能:将x射线以体素为voxel_size写入image。
数据成员:
1),
2),
*/
// Creates X-ray cuts through the points with pixels being 'voxel_size' big.
class XRayPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"write_xray_image";
XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
const std::vector<mapping::Floor>& floors,
const string& output_filename,
FileWriterFactory file_writer_factory,
PointsProcessor* next);
static std::unique_ptr<XRayPointsProcessor> FromDictionary(
const mapping::proto::Trajectory& trajectory,
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~XRayPointsProcessor() override {}
//将点云写入.png文件，然后流水到下一处理器
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
Eigen::AlignedBox3i bounding_box() const { return bounding_box_; }
private:
struct ColumnData {
double sum_r = 0.;
double sum_g = 0.;
double sum_b = 0.;
uint32_t count = 0;
};
struct Aggregation {
mapping_3d::HybridGridBase<bool> voxels;
std::map<std::pair<int, int>, ColumnData> column_data;
};
void WriteVoxels(const Aggregation& aggregation,
FileWriter* const file_writer);
void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
Aggregation* aggregation);
PointsProcessor* const next_;
FileWriterFactory file_writer_factory_; //负责文件写入的工厂
// If empty, we do not separate into floors.
std::vector<mapping::Floor> floors_; //楼层
const string output_filename_; //文件名称
const transform::Rigid3f transform_; //点云涉及的变换
// Only has one entry if we do not separate into floors.
std::vector<Aggregation> aggregations_; //聚集数据
// Bounding box containing all cells with data in all 'aggregations_'.
Eigen::AlignedBox3i bounding_box_; //边界框
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
================================================
FILE: io/xyz_writing_points_processor.cc
================================================
#include "cartographer/io/xyz_writing_points_processor.h"
#include <iomanip>
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace io {
namespace {
/*
将x,y,x以字符串形式写入FileWriter
*/
void WriteXyzPoint(const Eigen::Vector3f& point,
FileWriter* const file_writer) {
std::ostringstream stream;
stream << std::setprecision(6);
stream << point.x() << " " << point.y() << " " << point.z() << "\n";
const string out = stream.str();
CHECK(file_writer->Write(out.data(), out.size()));
}
} // namespace
/*
构造函数初始化
*/
XyzWriterPointsProcessor::XyzWriterPointsProcessor(
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
: next_(next), file_writer_(std::move(file_writer)) {}
/*
根据filename返回指向文件名的file_writer_factory
*/
std::unique_ptr<XyzWriterPointsProcessor>
XyzWriterPointsProcessor::FromDictionary
(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next)
{
return common::make_unique<XyzWriterPointsProcessor>(
file_writer_factory(dictionary->GetString("filename")), next);
}
/*刷新缓冲*/
PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
CHECK(file_writer_->Close()) << "Closing XYZ file failed.";
switch (next_->Flush()) {
case FlushResult::kFinished:
return FlushResult::kFinished;
case FlushResult::kRestartStream:
LOG(FATAL) << "XYZ generation must be configured to occur after any "
"stages that require multiple passes.";
}
LOG(FATAL);
}
/*
主逻辑,以{x,y,z}写入文件
*/
void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
for (const Eigen::Vector3f& point : batch->points) {
WriteXyzPoint(point, file_writer_.get());
}
next_->Process(std::move(batch));
}
} // namespace io
} // namespace cartographer
================================================
FILE: io/xyz_writing_points_processor.h
================================================
#ifndef CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
#include <fstream>
#include <memory>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
/*
XyzWriterPointsProcessor将points以{x,y,z}形式写入 FileWriter
*/
// Writes ASCII xyz points.
class XyzWriterPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "write_xyz";
XyzWriterPointsProcessor(std::unique_ptr<FileWriter>, PointsProcessor* next);
static std::unique_ptr<XyzWriterPointsProcessor> FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~XyzWriterPointsProcessor() override {}
XyzWriterPointsProcessor(const XyzWriterPointsProcessor&) = delete;
XyzWriterPointsProcessor& operator=(const XyzWriterPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
PointsProcessor* const next_;
std::unique_ptr<FileWriter> file_writer_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
================================================
FILE: kalman_filter/gaussian_distribution.h
================================================
#ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace cartographer {
namespace kalman_filter {
/*
高斯分布类
构造函数是N*1的均值矩阵和N*N的协方差矩阵
*/
template <typename T, int N>
class GaussianDistribution {
public:
GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
const Eigen::Matrix<T, N, N>& covariance)
: mean_(mean), covariance_(covariance) {}
const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }
const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }
private:
Eigen::Matrix<T, N, 1> mean_; //N*1，均值
Eigen::Matrix<T, N, N> covariance_; //N*N。协方差
};
/*
重载+加号操作符,
高斯+高斯=对应均值+均值,对应协方差+协方差
返回值：新高斯对象
高斯分布性质:
https://zh.wikipedia.org/wiki/%E6%AD%A3%E6%80%81%E5%88%86%E5%B8%83
*/
template <typename T, int N>
GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
const GaussianDistribution<T, N>& rhs) {
return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
lhs.GetCovariance() + rhs.GetCovariance());
}
/*
重载乘法运算符
1,矩阵N*M
2,高斯分布M*M
返回值:高斯分布：N*N
*/
template <typename T, int N, int M>
GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
const GaussianDistribution<T, M>& rhs) {
return GaussianDistribution<T, N>(
lhs * rhs.GetMean(), // N*M || M*1 -> N*1
lhs * rhs.GetCovariance() * lhs.transpose()); // N*M ||M*M || M*N -> N*N
}
} // namespace kalman_filter
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
================================================
FILE: kalman_filter/gaussian_distribution_test.cc
================================================
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace kalman_filter {
namespace {
TEST(GaussianDistributionTest, testConstructor) {
Eigen::Matrix2d covariance;//2*2
covariance << 1., 2., 3., 4.;
GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),
//均值是0,1
covariance);
EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);//0列的的均值是0
EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);
EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);//0行1列的方差是2
}
} // namespace
} // namespace kalman_filter
} // namespace cartographer
================================================
FILE: kalman_filter/pose_tracker.cc
================================================
#include "cartographer/kalman_filter/pose_tracker.h"
#include <cmath>
#include <limits>
#include <utility>
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
namespace cartographer {
namespace kalman_filter {
namespace {
/*定义函数AddDelta,当前状态State加上delta.
*/
PoseTracker::State AddDelta(const PoseTracker::State& state,
const PoseTracker::State& delta) {
PoseTracker::State new_state = state + delta;
const Eigen::Quaterniond orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
state[PoseTracker::kMapOrientationY],
state[PoseTracker::kMapOrientationZ]));
const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
delta[PoseTracker::kMapOrientationY],
delta[PoseTracker::kMapOrientationZ]);
CHECK_LT(rotation_vector.norm(), M_PI / 2.)
<< "Sigma point is far from the mean, recovered delta may be incorrect.";
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(rotation_vector);
const Eigen::Vector3d new_orientation =
transform::RotationQuaternionToAngleAxisVector(orientation * rotation);
new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
return new_state;
}
/*定义ComputeDelta()函数,计算State的差值delta
*/
PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
const PoseTracker::State& target) {
PoseTracker::State delta = target - origin;
const Eigen::Quaterniond origin_orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
origin[PoseTracker::kMapOrientationY],
origin[PoseTracker::kMapOrientationZ]));
const Eigen::Quaterniond target_orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
target[PoseTracker::kMapOrientationY],
target[PoseTracker::kMapOrientationZ]));
const Eigen::Vector3d rotation =
transform::RotationQuaternionToAngleAxisVector(
origin_orientation.inverse() * target_orientation);
delta[PoseTracker::kMapOrientationX] = rotation.x();
delta[PoseTracker::kMapOrientationY] = rotation.y();
delta[PoseTracker::kMapOrientationZ] = rotation.z();
return delta;
}
// Build a model matrix for the given time delta.返回delta时间后的状态
PoseTracker::State ModelFunction(const PoseTracker::State& state,
const double delta_t) {
CHECK_GT(delta_t, 0.);
PoseTracker::State new_state;
new_state[PoseTracker::kMapPositionX] = //x=x+delta * v
state[PoseTracker::kMapPositionX] +
delta_t * state[PoseTracker::kMapVelocityX];
new_state[PoseTracker::kMapPositionY] = //y=y+delta* v
state[PoseTracker::kMapPositionY] +
delta_t * state[PoseTracker::kMapVelocityY];
new_state[PoseTracker::kMapPositionZ] = //z=z+ delta* v
state[PoseTracker::kMapPositionZ] +
delta_t * state[PoseTracker::kMapVelocityZ];
new_state[PoseTracker::kMapOrientationX] =//不变
state[PoseTracker::kMapOrientationX];
new_state[PoseTracker::kMapOrientationY] =
state[PoseTracker::kMapOrientationY];
new_state[PoseTracker::kMapOrientationZ] =
state[PoseTracker::kMapOrientationZ];
new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];//不变
new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY];
new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ];
return new_state;
}
} // namespace
/*
定义乘法操作,C=A*B
*/
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance) {
GaussianDistribution<double, 6> distribution( // 6行1列
Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);
Eigen::Matrix<double, 6, 6> linear_transform; //6行6列
linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), transform.rotation().matrix();
return {transform * pose_and_covariance.pose,
(linear_transform * distribution).GetCovariance()};
}
/*
trajectory_builder_3d.lua:
pose_tracker = {
orientation_model_variance = 5e-3,
position_model_variance = 0.00654766,
velocity_model_variance = 0.53926,
-- This disables gravity alignment in local SLAM.
imu_gravity_time_constant = 1e9,
imu_gravity_variance = 0,
num_odometry_states = 1,
},
*/
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::PoseTrackerOptions options;
options.set_position_model_variance(
parameter_dictionary->GetDouble("position_model_variance"));
options.set_orientation_model_variance(
parameter_dictionary->GetDouble("orientation_model_variance"));
options.set_velocity_model_variance(
parameter_dictionary->GetDouble("velocity_model_variance"));
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_imu_gravity_variance(
parameter_dictionary->GetDouble("imu_gravity_variance"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
return options;
}
PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
State initial_state = State::Zero();
// We are certain about the complete state at the beginning. We define the
// initial pose to be at the origin and axis aligned. Additionally, we claim
// that we are not moving.
StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
return Distribution(initial_state, initial_covariance);
}
PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
const common::Time time)
: options_(options),
time_(time),
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
imu_tracker_(options.imu_gravity_time_constant(), time),
odometry_state_tracker_(options.num_odometry_states()) {}
PoseTracker::~PoseTracker() {}
PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
Predict(time);
return kalman_filter_.GetBelief();
}
void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
transform::Rigid3d* pose,
PoseCovariance* covariance) {
const Distribution belief = GetBelief(time);
*pose = RigidFromState(belief.GetMean());
static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
*covariance = belief.GetCovariance().block<6, 6>(0, 0);
covariance->block<2, 2>(3, 3) +=
options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
}
const PoseTracker::Distribution PoseTracker::BuildModelNoise(
const double delta_t) const {
// Position is constant, but orientation changes.
StateCovariance model_noise = StateCovariance::Zero();
model_noise.diagonal() <<
// Position in map.
options_.position_model_variance() * delta_t,
options_.position_model_variance() * delta_t,
options_.position_model_variance() * delta_t,
// Orientation in map.
options_.orientation_model_variance() * delta_t,
options_.orientation_model_variance() * delta_t,
options_.orientation_model_variance() * delta_t,
// Linear velocities in map.
options_.velocity_model_variance() * delta_t,
options_.velocity_model_variance() * delta_t,
options_.velocity_model_variance() * delta_t;
return Distribution(State::Zero(), model_noise);
}
void PoseTracker::Predict(const common::Time time) {
imu_tracker_.Advance(time);
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
if (delta_t == 0.) {
return;
}
kalman_filter_.Predict(
[this, delta_t](const State& state) -> State {
return ModelFunction(state, delta_t);
},
BuildModelNoise(delta_t));
time_ = time;
}
void PoseTracker::AddImuLinearAccelerationObservation(
const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
imu_tracker_.Advance(time);
imu_tracker_.AddImuLinearAccelerationObservation(imu_linear_acceleration);
Predict(time);
}
void PoseTracker::AddImuAngularVelocityObservation(
const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
imu_tracker_.Advance(time);
imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity);
Predict(time);
}
void PoseTracker::AddPoseObservation(const common::Time time,
const transform::Rigid3d& pose,
const PoseCovariance& covariance) {
Predict(time);
// Noise covariance is taken directly from the input values.
const GaussianDistribution<double, 6> delta(
Eigen::Matrix<double, 6, 1>::Zero(), covariance);
kalman_filter_.Observe<6>(
[this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
const transform::Rigid3d state_pose = RigidFromState(state);
const Eigen::Vector3d delta_orientation =
transform::RotationQuaternionToAngleAxisVector(
pose.rotation().inverse() * state_pose.rotation());
const Eigen::Vector3d delta_translation =
state_pose.translation() - pose.translation();
Eigen::Matrix<double, 6, 1> return_value;
return_value << delta_translation, delta_orientation;
return return_value;
},
delta);
}
// Updates from the odometer are in the odometer's map-like frame, called the
// 'odometry' frame. The odometer_pose converts data from the map frame
// into the odometry frame.
void PoseTracker::AddOdometerPoseObservation(
const common::Time time, const transform::Rigid3d& odometer_pose,
const PoseCovariance& covariance) {
if (!odometry_state_tracker_.empty()) {
const auto& previous_odometry_state = odometry_state_tracker_.newest();
const transform::Rigid3d delta =
previous_odometry_state.odometer_pose.inverse() * odometer_pose;
const transform::Rigid3d new_pose =
previous_odometry_state.state_pose * delta;
AddPoseObservation(time, new_pose, covariance);
}
const Distribution belief = GetBelief(time);
odometry_state_tracker_.AddOdometryState(
{time, odometer_pose, RigidFromState(belief.GetMean())});
}
const mapping::OdometryStateTracker::OdometryStates&
PoseTracker::odometry_states() const {
return odometry_state_tracker_.odometry_states();
}
transform::Rigid3d PoseTracker::RigidFromState(
const PoseTracker::State& state) {
return transform::Rigid3d(
Eigen::Vector3d(state[PoseTracker::kMapPositionX],
state[PoseTracker::kMapPositionY],
state[PoseTracker::kMapPositionZ]),
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
state[PoseTracker::kMapOrientationY],
state[PoseTracker::kMapOrientationZ])) *
imu_tracker_.orientation());
}
PoseCovariance BuildPoseCovariance(const double translational_variance,
const double rotational_variance) {
const Eigen::Matrix3d translational =
Eigen::Matrix3d::Identity() * translational_variance;
const Eigen::Matrix3d rotational =
Eigen::Matrix3d::Identity() * rotational_variance;
// clang-format off
PoseCovariance covariance;
covariance <<
translational, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotational;
// clang-format on
return covariance;
}
} // namespace kalman_filter
} // namespace cartographer
================================================
FILE: kalman_filter/pose_tracker.h
================================================
#ifndef CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
#include <deque>
#include <memory>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/odometry_state_tracker.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace kalman_filter {
typedef Eigen::Matrix3d Pose2DCovariance; //3*3矩阵
typedef Eigen::Matrix<double, 6, 6> PoseCovariance;// 6*6 矩阵
struct PoseAndCovariance {
transform::Rigid3d pose;
PoseCovariance covariance; //6*6
};
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance);
PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance);
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* parameter_dictionary);
/*
基于卡尔曼滤波的3维位置和方向估计
*/
// A Kalman filter for a 3D state of position and orientation.
// Includes functions to update from IMU and range data matches.
class PoseTracker {
public:
enum {
kMapPositionX = 0,//位置信息{X,Y,Z}
kMapPositionY,
kMapPositionZ,
kMapOrientationX,//方向信息,3
kMapOrientationY,
kMapOrientationZ,
kMapVelocityX, //速度信息,6
kMapVelocityY,
kMapVelocityZ,
kDimension //9, We terminate loops with this. 只追踪9个维度
};
using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;//9维的卡尔曼滤波
using State = KalmanFilter::StateType; //N*1矩阵
using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;//9*9
using Distribution = GaussianDistribution<double, kDimension>;
//参数类型的double,9*1的矩阵
// Create a new Kalman filter starting at the origin with a standard normal
// distribution at 'time'.
//在给定的time时刻初始化卡尔曼滤波参数
PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
virtual ~PoseTracker();
// Sets 'pose' and 'covariance' to the mean and covariance of the belief at
// 'time' which must be >= to the current time. Must not be nullptr.
//通过指针获取pose的旋转参数和covariance方差
void GetPoseEstimateMeanAndCovariance(common::Time time,
transform::Rigid3d* pose,
PoseCovariance* covariance);
// Updates from an IMU reading (in the IMU frame).
//根据imu观测值更新
void AddImuLinearAccelerationObservation(
common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
void AddImuAngularVelocityObservation(
common::Time time, const Eigen::Vector3d& imu_angular_velocity);
// Updates from a pose estimate in the map frame.
//根据map-frame的位姿估计更新
void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
const PoseCovariance& covariance);
// Updates from a pose estimate in the odometer's map-like frame.
//根据里程计的map-like frame位姿估计更新
void AddOdometerPoseObservation(common::Time time,
const transform::Rigid3d& pose,
const PoseCovariance& covariance);
common::Time time() const { return time_; } //最新有效时间
// Returns the belief at the 'time' which must be >= to the current time.
Distribution GetBelief(common::Time time); //未来某一时刻的状态估计值
const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
//imu的重力方向
Eigen::Quaterniond gravity_orientation() const {
return imu_tracker_.orientation();
}
private:
//返回初始状态的状态变量的高斯分布
// Returns the distribution required to initialize the KalmanFilter.
static Distribution KalmanFilterInit();
//建立零均值噪声模型
// Build a model noise distribution (zero mean) for the given time delta.
const Distribution BuildModelNoise(double delta_t) const;
// Predict the state forward in time. This is a no-op if 'time' has not moved
// forward.
//根据当前状态预测time时刻的状态
void Predict(common::Time time);
// Computes a pose combining the given 'state' with the 'imu_tracker_'
// orientation.
//结合 imu_tracker_和state,计算位姿pose的旋转变换。
transform::Rigid3d RigidFromState(const PoseTracker::State& state);
const proto::PoseTrackerOptions options_; //用于位姿估计的传感器特性
common::Time time_; //测量时间
KalmanFilter kalman_filter_; //卡尔曼滤波
mapping::ImuTracker imu_tracker_; //imu跟踪
mapping::OdometryStateTracker odometry_state_tracker_;//里程计跟踪
};
} // namespace kalman_filter
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
================================================
FILE: kalman_filter/pose_tracker_test.cc
================================================
#include "cartographer/kalman_filter/pose_tracker.h"
#include <random>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace kalman_filter {
namespace {
constexpr double kOdometerVariance = 1e-12;
using transform::IsNearly;
using transform::Rigid3d;
using ::testing::Not;
class PoseTrackerTest : public ::testing::Test {
protected:
PoseTrackerTest() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
orientation_model_variance = 1e-8,
position_model_variance = 1e-8,
velocity_model_variance = 1e-8,
imu_gravity_time_constant = 100.,
imu_gravity_variance = 1e-9,
num_odometry_states = 1,
}
)text");
const proto::PoseTrackerOptions options =
CreatePoseTrackerOptions(parameter_dictionary.get());
pose_tracker_ =
common::make_unique<PoseTracker>(options, common::FromUniversal(1000)); //创建卡尔曼滤波跟踪器
}
std::unique_ptr<PoseTracker> pose_tracker_;
};
TEST_F(PoseTrackerTest, SaveAndRestore) {
std::vector<Rigid3d> poses(3);
std::vector<PoseCovariance> covariances(3);
//0号，获取t=1500处的预测值
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
&poses[0], &covariances[0]);
//0号，添加t=2000的测量值
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
PoseTracker copy_of_pose_tracker = *pose_tracker_;//默认copy构造函数，1号
const Eigen::Vector3d observation(2, 0, 8);
//0号，添加t=3000的测量值
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
//0号，获取t=3500的预测值
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
&poses[1], &covariances[1]);
//1号，添加t=3000的测量值
copy_of_pose_tracker.AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
//1号，获取t=3500的测量值
copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
common::FromUniversal(3500), &poses[2], &covariances[2]);
EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
//相同的测量，导致相同的结果
EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
}
TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::seconds(5);
//循环300次，添加测量值
pose_tracker_->AddImuLinearAccelerationObservation(
time, Eigen::Vector3d(0., 0., 10.));
}
{
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
//expected.coeffs():0,0,0,1
//预测值和真实值相等
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
for (int i = 0; i < 300; ++i) {
time += std::chrono::seconds(5);
pose_tracker_->AddImuLinearAccelerationObservation(
time, Eigen::Vector3d(0., 10., 0.));
}
time += std::chrono::milliseconds(5);
Rigid3d pose;
PoseCovariance covariance;
//获取预测值
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond(
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
//预测值与真实值比较
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddImuAngularVelocityObservation(time,
Eigen::Vector3d::Zero());
}
{
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
const double target_radians = M_PI / 2.;
const double num_observations = 300.;
const double angular_velocity = target_radians / (num_observations * 5e-3);
for (int i = 0; i < num_observations; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddImuAngularVelocityObservation(
time, Eigen::Vector3d(angular_velocity, 0., 0.));
}
time += std::chrono::milliseconds(5);
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond(
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
TEST_F(PoseTrackerTest, AddPoseObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddPoseObservation(
time, Rigid3d::Identity(),
Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
}
{
Rigid3d actual;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
}
const Rigid3d expected =
Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(
M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(15);
pose_tracker_->AddPoseObservation(
time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
}
time += std::chrono::milliseconds(15);
Rigid3d actual;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
EXPECT_THAT(actual, IsNearly(expected, 1e-3));
}
TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
common::Time time = common::FromUniversal(0);
std::vector<Rigid3d> odometer_track;
odometer_track.push_back(Rigid3d::Identity());
odometer_track.push_back(
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
Rigid3d actual;
PoseCovariance unused_covariance;
for (const Rigid3d& pose : odometer_track) {
time += std::chrono::seconds(1);
pose_tracker_->AddOdometerPoseObservation(
time, pose, kOdometerVariance * PoseCovariance::Identity());
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
&unused_covariance);
EXPECT_THAT(actual, IsNearly(pose, 1e-2));
}
// Sanity check that the test has signal:
EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
}
} // namespace
} // namespace kalman_filter
} // namespace cartographer
================================================
FILE: kalman_filter/proto/pose_tracker_options.proto
================================================
// Copyright 2016 The Cartographer Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable
gitextract_6vczu7ti/
├── .gitignore
├── README.md
├── common/
│ ├── blocking_queue.h
│ ├── blocking_queue_test.cc
│ ├── ceres_solver_options.cc
│ ├── ceres_solver_options.h
│ ├── config.h
│ ├── config.h.cmake
│ ├── configuration_file_resolver.cc
│ ├── configuration_file_resolver.h
│ ├── fixed_ratio_sampler.cc
│ ├── fixed_ratio_sampler.h
│ ├── fixed_ratio_sampler_test.cc
│ ├── histogram.cc
│ ├── histogram.h
│ ├── lua.h
│ ├── lua_parameter_dictionary.cc
│ ├── lua_parameter_dictionary.h
│ ├── lua_parameter_dictionary_test.cc
│ ├── lua_parameter_dictionary_test_helpers.h
│ ├── make_unique.h
│ ├── make_unique_test.cc
│ ├── math.h
│ ├── math_test.cc
│ ├── mutex.h
│ ├── port.h
│ ├── port_le_test.cpp
│ ├── proto/
│ │ └── ceres_solver_options.proto
│ ├── rate_timer.h
│ ├── rate_timer_test.cc
│ ├── thread_pool.cc
│ ├── thread_pool.h
│ ├── time.cc
│ └── time.h
├── ground_truth/
│ └── compute_relations_metrics_main.cc
├── io/
│ ├── cairo_types.h
│ ├── coloring_points_processor.cc
│ ├── coloring_points_processor.h
│ ├── counting_points_processor.cc
│ ├── counting_points_processor.h
│ ├── file_writer.cc
│ ├── file_writer.h
│ ├── fixed_ratio_sampling_points_processor.cc
│ ├── fixed_ratio_sampling_points_processor.h
│ ├── intensity_to_color_points_processor.cc
│ ├── intensity_to_color_points_processor.h
│ ├── min_max_range_filtering_points_processor.cc
│ ├── min_max_range_filtering_points_processor.h
│ ├── null_points_processor.h
│ ├── outlier_removing_points_processor.cc
│ ├── outlier_removing_points_processor.h
│ ├── pcd_writing_points_processor.cc
│ ├── pcd_writing_points_processor.h
│ ├── ply_writing_points_processor.cc
│ ├── ply_writing_points_processor.h
│ ├── points_batch.cc
│ ├── points_batch.h
│ ├── points_processor.h
│ ├── points_processor_pipeline_builder.cc
│ ├── points_processor_pipeline_builder.h
│ ├── proto_stream.cc
│ ├── proto_stream.h
│ ├── proto_stream_test.cc
│ ├── xray_points_processor.cc
│ ├── xray_points_processor.h
│ ├── xyz_writing_points_processor.cc
│ └── xyz_writing_points_processor.h
├── kalman_filter/
│ ├── gaussian_distribution.h
│ ├── gaussian_distribution_test.cc
│ ├── pose_tracker.cc
│ ├── pose_tracker.h
│ ├── pose_tracker_test.cc
│ ├── proto/
│ │ └── pose_tracker_options.proto
│ ├── unscented_kalman_filter.h
│ └── unscented_kalman_filter_test.cc
├── mapping/
│ ├── collated_trajectory_builder.cc
│ ├── collated_trajectory_builder.h
│ ├── detect_floors.cc
│ ├── detect_floors.h
│ ├── global_trajectory_builder_interface.h
│ ├── id.h
│ ├── imu_tracker.cc
│ ├── imu_tracker.h
│ ├── map_builder.cc
│ ├── map_builder.h
│ ├── odometry_state_tracker.cc
│ ├── odometry_state_tracker.h
│ ├── probability_values.cc
│ ├── probability_values.h
│ ├── probability_values_test.cc
│ ├── proto/
│ │ ├── map_builder_options.proto
│ │ ├── sparse_pose_graph.proto
│ │ ├── sparse_pose_graph_options.proto
│ │ ├── submap_visualization.proto
│ │ ├── trajectory.proto
│ │ ├── trajectory_builder_options.proto
│ │ └── trajectory_connectivity.proto
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem_options.cc
│ │ ├── optimization_problem_options.h
│ │ └── proto/
│ │ ├── constraint_builder_options.proto
│ │ └── optimization_problem_options.proto
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── submaps.cc
│ ├── submaps.h
│ ├── submaps_test.cc
│ ├── trajectory_builder.cc
│ ├── trajectory_builder.h
│ ├── trajectory_connectivity.cc
│ ├── trajectory_connectivity.h
│ ├── trajectory_connectivity_test.cc
│ └── trajectory_node.h
├── mapping_2d/
│ ├── global_trajectory_builder.cc
│ ├── global_trajectory_builder.h
│ ├── local_trajectory_builder.cc
│ ├── local_trajectory_builder.h
│ ├── local_trajectory_builder_options.cc
│ ├── local_trajectory_builder_options.h
│ ├── map_limits.h
│ ├── map_limits_test.cc
│ ├── probability_grid.h
│ ├── probability_grid_test.cc
│ ├── proto/
│ │ ├── cell_limits.proto
│ │ ├── local_trajectory_builder_options.proto
│ │ ├── map_limits.proto
│ │ ├── probability_grid.proto
│ │ ├── range_data_inserter_options.proto
│ │ └── submaps_options.proto
│ ├── range_data_inserter.cc
│ ├── range_data_inserter.h
│ ├── range_data_inserter_test.cc
│ ├── ray_casting.cc
│ ├── ray_casting.h
│ ├── scan_matching/
│ │ ├── ceres_scan_matcher.cc
│ │ ├── ceres_scan_matcher.h
│ │ ├── ceres_scan_matcher_test.cc
│ │ ├── correlative_scan_matcher.cc
│ │ ├── correlative_scan_matcher.h
│ │ ├── correlative_scan_matcher_test.cc
│ │ ├── fast_correlative_scan_matcher.cc
│ │ ├── fast_correlative_scan_matcher.h
│ │ ├── fast_correlative_scan_matcher_test.cc
│ │ ├── fast_global_localizer.cc
│ │ ├── fast_global_localizer.h
│ │ ├── occupied_space_cost_functor.h
│ │ ├── proto/
│ │ │ ├── ceres_scan_matcher_options.proto
│ │ │ ├── fast_correlative_scan_matcher_options.proto
│ │ │ └── real_time_correlative_scan_matcher_options.proto
│ │ ├── real_time_correlative_scan_matcher.cc
│ │ ├── real_time_correlative_scan_matcher.h
│ │ ├── real_time_correlative_scan_matcher_test.cc
│ │ ├── rotation_delta_cost_functor.h
│ │ └── translation_delta_cost_functor.h
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem.cc
│ │ ├── optimization_problem.h
│ │ └── spa_cost_function.h
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── sparse_pose_graph_test.cc
│ ├── submaps.cc
│ ├── submaps.h
│ ├── submaps_test.cc
│ ├── xy_index.h
│ └── xy_index_test.cc
├── mapping_3d/
│ ├── acceleration_cost_function.h
│ ├── ceres_pose.cc
│ ├── ceres_pose.h
│ ├── global_trajectory_builder.cc
│ ├── global_trajectory_builder.h
│ ├── hybrid_grid.h
│ ├── hybrid_grid_test.cc
│ ├── imu_integration.cc
│ ├── imu_integration.h
│ ├── kalman_local_trajectory_builder.cc
│ ├── kalman_local_trajectory_builder.h
│ ├── kalman_local_trajectory_builder_options.cc
│ ├── kalman_local_trajectory_builder_options.h
│ ├── kalman_local_trajectory_builder_test.cc
│ ├── local_trajectory_builder.cc
│ ├── local_trajectory_builder.h
│ ├── local_trajectory_builder_interface.h
│ ├── local_trajectory_builder_options.cc
│ ├── local_trajectory_builder_options.h
│ ├── motion_filter.cc
│ ├── motion_filter.h
│ ├── motion_filter_test.cc
│ ├── optimizing_local_trajectory_builder.cc
│ ├── optimizing_local_trajectory_builder.h
│ ├── optimizing_local_trajectory_builder_options.cc
│ ├── optimizing_local_trajectory_builder_options.h
│ ├── proto/
│ │ ├── hybrid_grid.proto
│ │ ├── kalman_local_trajectory_builder_options.proto
│ │ ├── local_trajectory_builder_options.proto
│ │ ├── motion_filter_options.proto
│ │ ├── optimizing_local_trajectory_builder_options.proto
│ │ ├── range_data_inserter_options.proto
│ │ └── submaps_options.proto
│ ├── range_data_inserter.cc
│ ├── range_data_inserter.h
│ ├── range_data_inserter_test.cc
│ ├── rotation_cost_function.h
│ ├── scan_matching/
│ │ ├── ceres_scan_matcher.cc
│ │ ├── ceres_scan_matcher.h
│ │ ├── ceres_scan_matcher_test.cc
│ │ ├── fast_correlative_scan_matcher.cc
│ │ ├── fast_correlative_scan_matcher.h
│ │ ├── fast_correlative_scan_matcher_test.cc
│ │ ├── interpolated_grid.h
│ │ ├── interpolated_grid_test.cc
│ │ ├── occupied_space_cost_functor.h
│ │ ├── precomputation_grid.cc
│ │ ├── precomputation_grid.h
│ │ ├── precomputation_grid_test.cc
│ │ ├── proto/
│ │ │ ├── ceres_scan_matcher_options.proto
│ │ │ └── fast_correlative_scan_matcher_options.proto
│ │ ├── real_time_correlative_scan_matcher.cc
│ │ ├── real_time_correlative_scan_matcher.h
│ │ ├── real_time_correlative_scan_matcher_test.cc
│ │ ├── rotation_delta_cost_functor.h
│ │ ├── rotational_scan_matcher.cc
│ │ ├── rotational_scan_matcher.h
│ │ └── translation_delta_cost_functor.h
│ ├── sparse_pose_graph/
│ │ ├── constraint_builder.cc
│ │ ├── constraint_builder.h
│ │ ├── optimization_problem.cc
│ │ ├── optimization_problem.h
│ │ ├── optimization_problem_test.cc
│ │ └── spa_cost_function.h
│ ├── sparse_pose_graph.cc
│ ├── sparse_pose_graph.h
│ ├── submaps.cc
│ ├── submaps.h
│ └── translation_cost_function.h
├── sensor/
│ ├── collator.cc
│ ├── collator.h
│ ├── collator_test.cc
│ ├── compressed_point_cloud.cc
│ ├── compressed_point_cloud.h
│ ├── compressed_point_cloud_test.cc
│ ├── configuration.cc
│ ├── configuration.h
│ ├── data.h
│ ├── ordered_multi_queue.cc
│ ├── ordered_multi_queue.h
│ ├── ordered_multi_queue_le_test.cc
│ ├── ordered_multi_queue_test.cc
│ ├── point_cloud.cc
│ ├── point_cloud.h
│ ├── point_cloud_test.cc
│ ├── proto/
│ │ ├── adaptive_voxel_filter_options.proto
│ │ ├── configuration.proto
│ │ └── sensor.proto
│ ├── range_data.cc
│ ├── range_data.h
│ ├── range_data_test.cc
│ ├── voxel_filter.cc
│ ├── voxel_filter.h
│ └── voxel_filter_test.cc
└── transform/
├── proto/
│ └── transform.proto
├── rigid_transform.cc
├── rigid_transform.h
├── rigid_transform_le2_grid2_test.cc
├── rigid_transform_test_helpers.h
├── transform.cc
├── transform.h
├── transform_interpolation_buffer.cc
├── transform_interpolation_buffer.h
├── transform_interpolation_buffer_test.cc
└── transform_test.cc
SYMBOL INDEX (824 symbols across 236 files)
FILE: common/blocking_queue.h
function namespace (line 24) | namespace cartographer {
FILE: common/blocking_queue_test.cc
type cartographer (line 11) | namespace cartographer {
type common (line 12) | namespace common {
function TEST (line 15) | TEST(BlockingQueueTest, testPushPeekPop) {
function TEST (line 31) | TEST(BlockingQueueTest, testPushPopSharedPtr) {
function TEST (line 39) | TEST(BlockingQueueTest, testPopWithTimeout) {
function TEST (line 45) | TEST(BlockingQueueTest, testPushWithTimeout) {
function TEST (line 57) | TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) {
function TEST (line 70) | TEST(BlockingQueueTest, testBlockingPop) {
function TEST (line 85) | TEST(BlockingQueueTest, testBlockingPopWithTimeout) {
FILE: common/ceres_solver_options.cc
type cartographer (line 19) | namespace cartographer {
type common (line 20) | namespace common {
function CreateCeresSolverOptionsProto (line 25) | proto::CeresSolverOptions CreateCeresSolverOptionsProto(
function CreateCeresSolverOptions (line 42) | ceres::Solver::Options CreateCeresSolverOptions(
FILE: common/ceres_solver_options.h
function namespace (line 24) | namespace cartographer {
FILE: common/config.h
function namespace (line 20) | namespace cartographer {
FILE: common/configuration_file_resolver.cc
type cartographer (line 26) | namespace cartographer {
type common (line 27) | namespace common {
function string (line 36) | string ConfigurationFileResolver::GetFullPathOrDie(const string& bas...
function string (line 48) | string ConfigurationFileResolver::GetFileContentOrDie(const string& ...
FILE: common/configuration_file_resolver.h
function namespace (line 25) | namespace cartographer {
FILE: common/fixed_ratio_sampler.cc
type cartographer (line 5) | namespace cartographer {
type common (line 6) | namespace common {
function string (line 23) | string FixedRatioSampler::DebugString() {
FILE: common/fixed_ratio_sampler.h
function namespace (line 9) | namespace cartographer {
FILE: common/fixed_ratio_sampler_test.cc
type cartographer (line 7) | namespace cartographer {
type common (line 8) | namespace common {
function TEST (line 11) | TEST(FixedRatioSamplerTest, AlwaysTrue) {
function TEST (line 18) | TEST(FixedRatioSamplerTest, AlwaysFalse) {
function TEST (line 25) | TEST(FixedRatioSamplerTest, SometimesTrue) {
function TEST (line 32) | TEST(FixedRatioSamplerTest, FirstPulseIsTrue) {
FILE: common/histogram.cc
type cartographer (line 12) | namespace cartographer {
type common (line 13) | namespace common {
function string (line 19) | string Histogram::ToString(const int buckets) const {
FILE: common/histogram.h
function namespace (line 12) | namespace cartographer {
FILE: common/lua_parameter_dictionary.cc
type cartographer (line 52) | namespace cartographer {
type common (line 53) | namespace common {
function QuoteStringOnStack (line 65) | void QuoteStringOnStack(lua_State* L) { //加""引号,方便lua回调
function SetDictionaryInRegistry (line 86) | void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* d...
function LuaParameterDictionary (line 93) | LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
function CheckForLuaErrors (line 103) | void CheckForLuaErrors(lua_State* L, int status) {
function LuaChoose (line 108) | int LuaChoose(lua_State* L) {
function PushValue (line 122) | void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key...
function PushValue (line 123) | void PushValue(lua_State* L, const string& key) {
function GetValueFromLuaTable (line 130) | void GetValueFromLuaTable(lua_State* L, const T& key) {
function CheckTableIsAtTopOfStack (line 136) | void CheckTableIsAtTopOfStack(lua_State* L) {
function HasKeyOfType (line 142) | bool HasKeyOfType(lua_State* L, const T& key) {
function GetArrayValues (line 154) | void GetArrayValues(lua_State* L, const std::function<void()>& pop_v...
function string (line 248) | string LuaParameterDictionary::GetString(const string& key) {
function string (line 254) | string LuaParameterDictionary::PopString(Quoted quoted) const {
function string (line 321) | string LuaParameterDictionary::DoToString(const string& indent) const {
function string (line 394) | string LuaParameterDictionary::ToString() const { return DoToString(...
FILE: common/lua_parameter_dictionary.h
function namespace (line 30) | namespace common {
FILE: common/lua_parameter_dictionary_test.cc
type cartographer (line 27) | namespace cartographer {
type common (line 28) | namespace common {
function MakeNonReferenceCounted (line 32) | std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
class LuaParameterDictionaryTest (line 38) | class LuaParameterDictionaryTest : public ::testing::Test {
method ReferenceAllKeysAsIntegers (line 40) | void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
function TEST_F (line 47) | TEST_F(LuaParameterDictionaryTest, GetInt) {
function TEST_F (line 52) | TEST_F(LuaParameterDictionaryTest, GetString) {
function TEST_F (line 57) | TEST_F(LuaParameterDictionaryTest, GetDouble) {
function TEST_F (line 62) | TEST_F(LuaParameterDictionaryTest, GetBoolTrue) {
function TEST_F (line 67) | TEST_F(LuaParameterDictionaryTest, GetBoolFalse) {
function TEST_F (line 72) | TEST_F(LuaParameterDictionaryTest, GetDictionary) {
function TEST_F (line 91) | TEST_F(LuaParameterDictionaryTest, GetKeys) {
function TEST_F (line 103) | TEST_F(LuaParameterDictionaryTest, NonExistingKey) {
function TEST_F (line 109) | TEST_F(LuaParameterDictionaryTest, UintNegative) {
function TEST_F (line 115) | TEST_F(LuaParameterDictionaryTest, ToString) {
function TEST_F (line 162) | TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
function TEST_F (line 178) | TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
function TEST_F (line 188) | TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) {
function TEST_F (line 198) | TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) {
function TEST_F (line 208) | TEST_F(LuaParameterDictionaryTest, TestChooseTrue) {
function TEST_F (line 213) | TEST_F(LuaParameterDictionaryTest, TestChoseFalse) {
function TEST_F (line 218) | TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) {
FILE: common/lua_parameter_dictionary_test_helpers.h
function namespace (line 28) | namespace cartographer {
FILE: common/make_unique.h
function namespace (line 11) | namespace cartographer {
FILE: common/make_unique_test.cc
type cartographer (line 11) | namespace cartographer {
type common (line 12) | namespace common {
function TEST (line 13) | TEST(MAKE_UNIQUE ,make_unique) {
FILE: common/math.h
function namespace (line 16) | namespace cartographer {
function T (line 34) | T Power(T base, int exponent) {
function T (line 40) | T Pow2(T a) {
function DegToRad (line 45) | constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
function RadToDeg (line 48) | constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
FILE: common/math_test.cc
type cartographer (line 7) | namespace cartographer {
type common (line 8) | namespace common {
function TEST (line 11) | TEST(MathTest, testPower) {
function TEST (line 19) | TEST(MathTest, testPow2) {
function TEST (line 26) | TEST(MathTest, testDeg2rad) {
function TEST (line 31) | TEST(MathTest, testRad2deg) {
function TEST (line 36) | TEST(MathTest, testNormalizeAngleDifference) {
FILE: common/mutex.h
function namespace (line 15) | namespace cartographer {
FILE: common/port.h
function namespace (line 29) | namespace cartographer {
FILE: common/port_le_test.cpp
function main (line 11) | int main(int argc, char *argv[])
FILE: common/rate_timer.h
function namespace (line 26) | namespace cartographer {
FILE: common/rate_timer_test.cc
type cartographer (line 6) | namespace cartographer {
type common (line 7) | namespace common {
function TEST (line 10) | TEST(RateTimerTest, ComputeRate) {
type SimulatedClock (line 20) | struct SimulatedClock { /*模拟的时间clock类*/
method time_point (line 28) | static time_point now() noexcept { return time; }
function TEST (line 33) | TEST(RateTimerTest, ComputeWallTimeRateRatio) {
FILE: common/thread_pool.cc
type cartographer (line 12) | namespace cartographer {
type common (line 13) | namespace common {
FILE: common/thread_pool.h
function namespace (line 12) | namespace cartographer {
FILE: common/time.cc
type cartographer (line 7) | namespace cartographer {
type common (line 8) | namespace common {
function Duration (line 11) | Duration FromSeconds(const double seconds) {
function ToSeconds (line 17) | double ToSeconds(const Duration duration) {
function Time (line 25) | Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); }
function int64 (line 29) | int64 ToUniversal(const Time time) { return time.time_since_epoch()....
function FromMilliseconds (line 38) | common::Duration FromMilliseconds(const int64 milliseconds) {
FILE: common/time.h
function namespace (line 27) | namespace cartographer {
FILE: ground_truth/compute_relations_metrics_main.cc
type cartographer (line 26) | namespace cartographer {
type ground_truth (line 27) | namespace ground_truth {
function LookupPose (line 30) | transform::Rigid3d LookupPose(const transform::TransformInterpolatio...
type Error (line 41) | struct Error {
function Error (line 46) | Error ComputeError(const transform::Rigid3d& pose1,
function string (line 55) | string MeanAndStdDevString(const std::vector<double>& values) {
function string (line 71) | string StatisticsString(const std::vector<Error>& errors) {
function Run (line 97) | void Run(const string& trajectory_filename, const string& relations_...
function main (line 134) | int main(int argc, char** argv) {
FILE: io/cairo_types.h
function namespace (line 16) | namespace cartographer {
FILE: io/coloring_points_processor.cc
type cartographer (line 8) | namespace cartographer {
type io (line 9) | namespace io {
FILE: io/coloring_points_processor.h
function namespace (line 11) | namespace cartographer {
FILE: io/counting_points_processor.cc
type cartographer (line 7) | namespace cartographer {
type io (line 8) | namespace io {
FILE: io/counting_points_processor.h
function namespace (line 8) | namespace cartographer {
FILE: io/file_writer.cc
type cartographer (line 4) | namespace cartographer {
type io (line 5) | namespace io {
FILE: io/file_writer.h
function namespace (line 11) | namespace cartographer {
FILE: io/fixed_ratio_sampling_points_processor.cc
type cartographer (line 9) | namespace cartographer {
type io (line 10) | namespace io {
FILE: io/fixed_ratio_sampling_points_processor.h
function namespace (line 11) | namespace cartographer {
FILE: io/intensity_to_color_points_processor.cc
type cartographer (line 10) | namespace cartographer {
type io (line 11) | namespace io {
FILE: io/intensity_to_color_points_processor.h
function namespace (line 14) | namespace cartographer {
FILE: io/min_max_range_filtering_points_processor.cc
type cartographer (line 8) | namespace cartographer {
type io (line 9) | namespace io {
FILE: io/min_max_range_filtering_points_processor.h
function namespace (line 10) | namespace cartographer {
FILE: io/null_points_processor.h
function namespace (line 8) | namespace cartographer {
FILE: io/outlier_removing_points_processor.cc
type cartographer (line 9) | namespace cartographer {
type io (line 10) | namespace io {
FILE: io/outlier_removing_points_processor.h
function namespace (line 11) | namespace io {
FILE: io/pcd_writing_points_processor.cc
type cartographer (line 13) | namespace cartographer {
type io (line 14) | namespace io {
function WriteBinaryPcdHeader (line 40) | void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
function WriteBinaryPcdPointCoordinate (line 65) | void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
function WriteBinaryPcdPointColor (line 74) | void WriteBinaryPcdPointColor(const Color& color,
FILE: io/pcd_writing_points_processor.h
function namespace (line 17) | namespace cartographer {
FILE: io/ply_writing_points_processor.cc
type cartographer (line 13) | namespace cartographer {
type io (line 14) | namespace io {
function WriteBinaryPlyHeader (line 25) | void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
function WriteBinaryPlyPointCoordinate (line 48) | void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
function WriteBinaryPlyPointColor (line 60) | void WriteBinaryPlyPointColor(const Color& color,
FILE: io/ply_writing_points_processor.h
function namespace (line 6) | namespace cartographer {
FILE: io/points_batch.cc
type cartographer (line 4) | namespace cartographer {
type io (line 5) | namespace io {
function RemovePoints (line 23) | void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
FILE: io/points_batch.h
function namespace (line 12) | namespace cartographer {
FILE: io/points_processor.h
function namespace (line 9) | namespace cartographer {
FILE: io/points_processor_pipeline_builder.cc
type cartographer (line 19) | namespace cartographer {
type io (line 20) | namespace io {
function RegisterPlainPointsProcessor (line 37) | void RegisterPlainPointsProcessor(
function RegisterFileWritingPointsProcessor (line 60) | void RegisterFileWritingPointsProcessor(
function RegisterBuiltInPointsProcessors (line 83) | void RegisterBuiltInPointsProcessors(
FILE: io/points_processor_pipeline_builder.h
function namespace (line 15) | namespace cartographer {
FILE: io/proto_stream.cc
type cartographer (line 4) | namespace cartographer {
type io (line 5) | namespace io {
function WriteSizeAsLittleEndian (line 12) | void WriteSizeAsLittleEndian(size_t size, std::ostream* out) {
function ReadSizeAsLittleEndian (line 19) | bool ReadSizeAsLittleEndian(std::istream* in, size_t* size) {
FILE: io/proto_stream.h
function namespace (line 9) | namespace cartographer {
FILE: io/proto_stream_test.cc
type cartographer (line 13) | namespace cartographer {
type io (line 14) | namespace io {
class ProtoStreamTest (line 17) | class ProtoStreamTest : public ::testing::Test {
method SetUp (line 19) | void SetUp() override {
method TearDown (line 25) | void TearDown() override { remove(test_directory_.c_str()); }
function TEST_F (line 30) | TEST_F(ProtoStreamTest, WriteAndReadBack) {
FILE: io/xray_points_processor.cc
type cartographer (line 16) | namespace cartographer {
type io (line 17) | namespace io {
type PixelData (line 20) | struct PixelData {
function Mix (line 30) | double Mix(const double a, const double b, const double t) {
function cairo_status_t (line 35) | cairo_status_t CairoWriteCallback(void* const closure,
function WritePng (line 49) | void WritePng(const PixelDataMatrix& mat, FileWriter* const file_wri...
function ContainedIn (line 112) | bool ContainedIn(const common::Time& time,
FILE: io/xray_points_processor.h
function namespace (line 32) | namespace cartographer {
FILE: io/xyz_writing_points_processor.cc
type cartographer (line 8) | namespace cartographer {
type io (line 9) | namespace io {
function WriteXyzPoint (line 16) | void WriteXyzPoint(const Eigen::Vector3f& point,
FILE: io/xyz_writing_points_processor.h
function namespace (line 13) | namespace cartographer {
FILE: kalman_filter/gaussian_distribution.h
function namespace (line 9) | namespace cartographer {
FILE: kalman_filter/gaussian_distribution_test.cc
type cartographer (line 5) | namespace cartographer {
type kalman_filter (line 6) | namespace kalman_filter {
function TEST (line 9) | TEST(GaussianDistributionTest, testConstructor) {
FILE: kalman_filter/pose_tracker.cc
type cartographer (line 17) | namespace cartographer {
type kalman_filter (line 18) | namespace kalman_filter {
function AddDelta (line 24) | PoseTracker::State AddDelta(const PoseTracker::State& state,
function ComputeDelta (line 49) | PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
function ModelFunction (line 72) | PoseTracker::State ModelFunction(const PoseTracker::State& state,
function PoseAndCovariance (line 108) | PoseAndCovariance operator*(const transform::Rigid3d& transform,
function CreatePoseTrackerOptions (line 134) | proto::PoseTrackerOptions CreatePoseTrackerOptions(
function PoseCovariance (line 311) | PoseCovariance BuildPoseCovariance(const double translational_variance,
FILE: kalman_filter/pose_tracker.h
function namespace (line 21) | namespace cartographer {
FILE: kalman_filter/pose_tracker_test.cc
type cartographer (line 13) | namespace cartographer {
type kalman_filter (line 14) | namespace kalman_filter {
class PoseTrackerTest (line 23) | class PoseTrackerTest : public ::testing::Test {
method PoseTrackerTest (line 25) | PoseTrackerTest() {
function TEST_F (line 45) | TEST_F(PoseTrackerTest, SaveAndRestore) {
function TEST_F (line 82) | TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
function TEST_F (line 124) | TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
function TEST_F (line 164) | TEST_F(PoseTrackerTest, AddPoseObservation) {
function TEST_F (line 200) | TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
FILE: kalman_filter/unscented_kalman_filter.h
function namespace (line 18) | namespace cartographer {
function StateType (line 299) | StateType ComputeMean(const std::vector<StateType>& states) {
FILE: kalman_filter/unscented_kalman_filter_test.cc
type cartographer (line 7) | namespace cartographer {
type kalman_filter (line 8) | namespace kalman_filter {
function Scalar (line 12) | Eigen::Matrix<double, 1, 1> Scalar(double value) {
function g (line 18) | Eigen::Matrix<double, 2, 1> g(const Eigen::Matrix<double, 2, 1>& sta...
function h (line 27) | Eigen::Matrix<double, 1, 1> h(const Eigen::Matrix<double, 2, 1>& sta...
function TEST (line 31) | TEST(KalmanFilterTest, testConstructor) {
function TEST (line 44) | TEST(KalmanFilterTest, testPredict) {
function TEST (line 61) | TEST(KalmanFilterTest, testObserve) {
FILE: mapping/collated_trajectory_builder.cc
type cartographer (line 7) | namespace cartographer {
type mapping (line 8) | namespace mapping {
function Submaps (line 37) | const Submaps* CollatedTrajectoryBuilder::submaps() const {
FILE: mapping/collated_trajectory_builder.h
function namespace (line 19) | namespace cartographer {
FILE: mapping/detect_floors.cc
type cartographer (line 12) | namespace cartographer {
type mapping (line 13) | namespace mapping {
type Span (line 28) | struct Span {
function LevelFind (line 40) | int LevelFind(const int i, const Levels& levels) {
function LevelUnion (line 49) | void LevelUnion(int i, int j, Levels* levels) {
function InsertSorted (line 55) | void InsertSorted(const double val, std::vector<double>* vals) {
function Median (line 59) | double Median(const std::vector<double>& sorted) {
function SliceByAltitudeChange (line 66) | std::vector<Span> SliceByAltitudeChange(const proto::Trajectory& tra...
function SpanLength (line 84) | double SpanLength(const proto::Trajectory& trajectory, const Span& s...
function IsShort (line 97) | bool IsShort(const proto::Trajectory& trajectory, const Span& span) {
function GroupSegmentsByAltitude (line 102) | void GroupSegmentsByAltitude(const proto::Trajectory& trajectory,
function FindFloors (line 114) | std::vector<Floor> FindFloors(const proto::Trajectory& trajectory,
function DetectFloors (line 184) | std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory) {
FILE: mapping/detect_floors.h
function namespace (line 8) | namespace cartographer {
FILE: mapping/global_trajectory_builder_interface.h
function namespace (line 15) | namespace cartographer {
FILE: mapping/id.h
function namespace (line 10) | namespace mapping {
FILE: mapping/imu_tracker.cc
type cartographer (line 11) | namespace cartographer {
type mapping (line 12) | namespace mapping {
FILE: mapping/imu_tracker.h
function namespace (line 8) | namespace cartographer {
FILE: mapping/map_builder.cc
type cartographer (line 20) | namespace cartographer {
type mapping (line 21) | namespace mapping {
function CreateMapBuilderOptions (line 62) | proto::MapBuilderOptions CreateMapBuilderOptions(
function TrajectoryBuilder (line 120) | TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
function string (line 137) | string MapBuilder::SubmapToProto(const int trajectory_id,
function SparsePoseGraph (line 168) | SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pos...
FILE: mapping/map_builder.h
function namespace (line 25) | namespace cartographer {
FILE: mapping/odometry_state_tracker.cc
type cartographer (line 6) | namespace cartographer {
type mapping (line 7) | namespace mapping {
function OdometryState (line 36) | const OdometryState& OdometryStateTracker::newest() const {
FILE: mapping/odometry_state_tracker.h
function namespace (line 10) | namespace cartographer {
FILE: mapping/probability_values.cc
type cartographer (line 4) | namespace cartographer {
type mapping (line 5) | namespace mapping {
function SlowValueToProbability (line 14) | float SlowValueToProbability(const uint16 value) {
function ComputeLookupTableToApplyOdds (line 71) | std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
FILE: mapping/probability_values.h
function namespace (line 12) | namespace cartographer {
FILE: mapping/probability_values_test.cc
type cartographer (line 5) | namespace cartographer {
type mapping (line 6) | namespace mapping {
function TEST (line 9) | TEST(ProbabilityValuesTest, OddsConversions) {
FILE: mapping/sparse_pose_graph.cc
type cartographer (line 9) | namespace cartographer {
type mapping (line 10) | namespace mapping {
function ToProto (line 12) | proto::SparsePoseGraph::Constraint::Tag ToProto(
function CreateSparsePoseGraphOptions (line 24) | proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
FILE: mapping/sparse_pose_graph.h
function namespace (line 17) | namespace cartographer {
FILE: mapping/sparse_pose_graph/constraint_builder.cc
type cartographer (line 10) | namespace cartographer {
type mapping (line 11) | namespace mapping {
type sparse_pose_graph (line 12) | namespace sparse_pose_graph {
function CreateConstraintBuilderOptions (line 14) | proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
FILE: mapping/sparse_pose_graph/constraint_builder.h
function namespace (line 8) | namespace cartographer {
FILE: mapping/sparse_pose_graph/optimization_problem_options.cc
type cartographer (line 6) | namespace cartographer {
type mapping (line 7) | namespace mapping {
type sparse_pose_graph (line 8) | namespace sparse_pose_graph {
function CreateOptimizationProblemOptions (line 28) | proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
FILE: mapping/sparse_pose_graph/optimization_problem_options.h
function namespace (line 9) | namespace cartographer {
FILE: mapping/submaps.cc
type cartographer (line 9) | namespace cartographer {
type mapping (line 10) | namespace mapping {
FILE: mapping/submaps.h
function namespace (line 18) | namespace cartographer {
FILE: mapping/submaps_test.cc
type cartographer (line 8) | namespace cartographer {
type mapping (line 9) | namespace mapping {
function Expit (line 14) | inline float Expit(float log_odds) { //求指数倍
function TEST (line 19) | TEST(SubmapsTest, LogOddsConversions) {
FILE: mapping/trajectory_builder.cc
type cartographer (line 7) | namespace cartographer {
type mapping (line 8) | namespace mapping {
function CreateTrajectoryBuilderOptions (line 11) | proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
FILE: mapping/trajectory_builder.h
function namespace (line 19) | namespace cartographer {
FILE: mapping/trajectory_connectivity.cc
type cartographer (line 10) | namespace cartographer {
type mapping (line 11) | namespace mapping {
function ToProto (line 83) | proto::TrajectoryConnectivity ToProto(
function FindConnectedComponent (line 99) | proto::TrajectoryConnectivity::ConnectedComponent FindConnectedCompo...
FILE: mapping/trajectory_connectivity.h
function namespace (line 12) | namespace cartographer {
FILE: mapping/trajectory_connectivity_test.cc
type cartographer (line 10) | namespace cartographer {
type mapping (line 11) | namespace mapping {
function TEST (line 16) | TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
function TEST (line 44) | TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) {
function TEST (line 50) | TEST(TrajectoryConnectivityTest, ConnectedComponents) {
function TEST (line 83) | TEST(TrajectoryConnectivityTest, ConnectionCount) {
FILE: mapping/trajectory_node.h
function namespace (line 13) | namespace cartographer {
FILE: mapping_2d/global_trajectory_builder.cc
type cartographer (line 19) | namespace cartographer {
type mapping_2d (line 20) | namespace mapping_2d {
function Submaps (line 31) | const Submaps* GlobalTrajectoryBuilder::submaps() const {
FILE: mapping_2d/global_trajectory_builder.h
function namespace (line 24) | namespace cartographer {
FILE: mapping_2d/local_trajectory_builder.cc
type cartographer (line 24) | namespace cartographer {
type mapping_2d (line 25) | namespace mapping_2d {
function Submaps (line 39) | const Submaps* LocalTrajectoryBuilder::submaps() const { return &sub...
FILE: mapping_2d/local_trajectory_builder.h
function namespace (line 35) | namespace cartographer {
FILE: mapping_2d/local_trajectory_builder_options.cc
type cartographer (line 25) | namespace cartographer {
type mapping_2d (line 26) | namespace mapping_2d {
function CreateLocalTrajectoryBuilderOptions (line 28) | proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOpt...
FILE: mapping_2d/local_trajectory_builder_options.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_2d/map_limits.h
function namespace (line 35) | namespace cartographer {
FILE: mapping_2d/map_limits_test.cc
type cartographer (line 21) | namespace cartographer {
type mapping_2d (line 22) | namespace mapping_2d {
function TEST (line 25) | TEST(MapLimitsTest, ToProto) {
function TEST (line 35) | TEST(MapLimitsTest, ProtoConstructor) {
function TEST (line 51) | TEST(MapLimitsTest, ConstructAndGet) {
FILE: mapping_2d/probability_grid.h
function namespace (line 37) | namespace mapping_2d {
function StartUpdate (line 72) | void StartUpdate() {
function SetProbability (line 82) | void SetProbability(const Eigen::Array2i& xy_index, const float probabil...
function ApplyLookupTable (line 96) | bool ApplyLookupTable(const Eigen::Array2i& xy_index,
function GetProbability (line 112) | float GetProbability(const Eigen::Array2i& xy_index) const {
function GetProbability (line 120) | float GetProbability(const double x, const double y) const {
function IsKnown (line 125) | bool IsKnown(const Eigen::Array2i& xy_index) const {
function ComputeCroppedLimits (line 132) | void ComputeCroppedLimits(Eigen::Array2i* const offset,
function GrowLimits (line 142) | void GrowLimits(const double x, const double y) {
FILE: mapping_2d/probability_grid_test.cc
type cartographer (line 24) | namespace cartographer {
type mapping_2d (line 25) | namespace mapping_2d {
function TEST (line 28) | TEST(ProbabilityGridTest, ProtoConstructor) {
function TEST (line 50) | TEST(ProbabilityGridTest, ToProto) {
function TEST (line 62) | TEST(ProbabilityGridTest, ApplyOdds) {
function TEST (line 114) | TEST(ProbabilityGridTest, GetProbability) {
function TEST (line 141) | TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
function TEST (line 180) | TEST(ProbabilityGridTest, CorrectCropping) {
FILE: mapping_2d/range_data_inserter.cc
type cartographer (line 27) | namespace cartographer {
type mapping_2d (line 28) | namespace mapping_2d {
function CreateRangeDataInserterOptions (line 30) | proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
FILE: mapping_2d/range_data_inserter.h
function namespace (line 31) | namespace cartographer {
FILE: mapping_2d/range_data_inserter_test.cc
type cartographer (line 27) | namespace cartographer {
type mapping_2d (line 28) | namespace mapping_2d {
class RangeDataInserterTest (line 31) | class RangeDataInserterTest : public ::testing::Test {
method RangeDataInserterTest (line 33) | RangeDataInserterTest()
method InsertPointCloud (line 46) | void InsertPointCloud() {
function TEST_F (line 63) | TEST_F(RangeDataInserterTest, InsertPointCloud) {
function TEST_F (line 103) | TEST_F(RangeDataInserterTest, ProbabilityProgression) {
FILE: mapping_2d/ray_casting.cc
type cartographer (line 19) | namespace cartographer {
type mapping_2d (line 20) | namespace mapping_2d {
function CastRay (line 30) | void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
function CastRays (line 150) | void CastRays(const sensor::RangeData& range_data, const MapLimits& ...
FILE: mapping_2d/ray_casting.h
function namespace (line 28) | namespace cartographer {
FILE: mapping_2d/scan_matching/ceres_scan_matcher.cc
type cartographer (line 33) | namespace cartographer {
type mapping_2d (line 34) | namespace mapping_2d {
type scan_matching (line 35) | namespace scan_matching {
function CreateCeresScanMatcherOptions (line 37) | proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
FILE: mapping_2d/scan_matching/ceres_scan_matcher.h
function namespace (line 30) | namespace cartographer {
FILE: mapping_2d/scan_matching/ceres_scan_matcher_test.cc
type cartographer (line 29) | namespace cartographer {
type mapping_2d (line 30) | namespace mapping_2d {
type scan_matching (line 31) | namespace scan_matching {
class CeresScanMatcherTest (line 34) | class CeresScanMatcherTest : public ::testing::Test {
method CeresScanMatcherTest (line 36) | CeresScanMatcherTest()
method TestFromInitialPose (line 61) | void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
function TEST_F (line 79) | TEST_F(CeresScanMatcherTest, testPerfectEstimate) {
function TEST_F (line 83) | TEST_F(CeresScanMatcherTest, testOptimizeAlongX) {
function TEST_F (line 87) | TEST_F(CeresScanMatcherTest, testOptimizeAlongY) {
function TEST_F (line 91) | TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) {
FILE: mapping_2d/scan_matching/correlative_scan_matcher.cc
type cartographer (line 23) | namespace cartographer {
type mapping_2d (line 24) | namespace mapping_2d {
type scan_matching (line 25) | namespace scan_matching {
function GenerateRotatedScans (line 93) | std::vector<sensor::PointCloud> GenerateRotatedScans(
function DiscretizeScans (line 111) | std::vector<DiscreteScan> DiscretizeScans(
FILE: mapping_2d/scan_matching/correlative_scan_matcher.h
function namespace (line 28) | namespace cartographer {
FILE: mapping_2d/scan_matching/correlative_scan_matcher_test.cc
type cartographer (line 22) | namespace cartographer {
type mapping_2d (line 23) | namespace mapping_2d {
type scan_matching (line 24) | namespace scan_matching {
function TEST (line 27) | TEST(SearchParameters, Construction) {
function TEST (line 43) | TEST(Candidate, Construction) {
function TEST (line 59) | TEST(GenerateRotatedScans, GenerateRotatedScans) {
function TEST (line 73) | TEST(DiscretizeScans, DiscretizeScans) {
FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
type cartographer (line 32) | namespace cartographer {
type mapping_2d (line 33) | namespace mapping_2d {
type scan_matching (line 34) | namespace scan_matching {
class SlidingWindowMaximum (line 41) | class SlidingWindowMaximum {
method AddValue (line 43) | void AddValue(const float value) {
method RemoveValue (line 51) | void RemoveValue(const float value) {
method GetMaximum (line 61) | float GetMaximum() const {
method CheckIsEmpty (line 68) | void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(...
function CreateFastCorrelativeScanMatcherOptions (line 78) | proto::FastCorrelativeScanMatcherOptions
function uint8 (line 161) | uint8 PrecomputationGrid::ComputeCellValue(const float probability...
class PrecomputationGridStack (line 170) | class PrecomputationGridStack {
method PrecomputationGridStack (line 172) | PrecomputationGridStack(
method PrecomputationGrid (line 189) | const PrecomputationGrid& Get(int index) {
method max_depth (line 193) | int max_depth() const { return precomputation_grids_.size() - 1; }
function Candidate (line 345) | Candidate FastCorrelativeScanMatcher::BranchAndBound(
FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher.h
function namespace (line 38) | namespace cartographer {
FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
type cartographer (line 32) | namespace cartographer {
type mapping_2d (line 33) | namespace mapping_2d {
type scan_matching (line 34) | namespace scan_matching {
function TEST (line 37) | TEST(PrecomputationGridTest, CorrectValues) {
function TEST (line 74) | TEST(PrecomputationGridTest, TinyProbabilityGrid) {
function CreateFastCorrelativeScanMatcherTestOptions (line 109) | proto::FastCorrelativeScanMatcherOptions
function CreateRangeDataInserterTestOptions (line 121) | mapping_2d::proto::RangeDataInserterOptions
function TEST (line 132) | TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
function TEST (line 179) | TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
FILE: mapping_2d/scan_matching/fast_global_localizer.cc
type cartographer (line 21) | namespace cartographer {
type mapping_2d (line 22) | namespace mapping_2d {
type scan_matching (line 23) | namespace scan_matching {
function PerformGlobalLocalization (line 25) | bool PerformGlobalLocalization(
FILE: mapping_2d/scan_matching/fast_global_localizer.h
function namespace (line 26) | namespace cartographer {
FILE: mapping_2d/scan_matching/occupied_space_cost_functor.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc
type cartographer (line 32) | namespace cartographer {
type mapping_2d (line 33) | namespace mapping_2d {
type scan_matching (line 34) | namespace scan_matching {
function CreateRealTimeCorrelativeScanMatcherOptions (line 36) | proto::RealTimeCorrelativeScanMatcherOptions
FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher.h
function namespace (line 48) | namespace cartographer {
FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
type cartographer (line 31) | namespace cartographer {
type mapping_2d (line 32) | namespace mapping_2d {
type scan_matching (line 33) | namespace scan_matching {
class RealTimeCorrelativeScanMatcherTest (line 36) | class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
method RealTimeCorrelativeScanMatcherTest (line 38) | RealTimeCorrelativeScanMatcherTest()
function TEST_F (line 84) | TEST_F(RealTimeCorrelativeScanMatcherTest,
function TEST_F (line 102) | TEST_F(RealTimeCorrelativeScanMatcherTest,
FILE: mapping_2d/scan_matching/rotation_delta_cost_functor.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_2d/scan_matching/translation_delta_cost_functor.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_2d/sparse_pose_graph.cc
type cartographer (line 38) | namespace cartographer {
type mapping_2d (line 39) | namespace mapping_2d {
FILE: mapping_2d/sparse_pose_graph.h
function namespace (line 42) | namespace cartographer {
FILE: mapping_2d/sparse_pose_graph/constraint_builder.cc
type cartographer (line 37) | namespace cartographer {
type mapping_2d (line 38) | namespace mapping_2d {
type sparse_pose_graph (line 39) | namespace sparse_pose_graph {
function ComputeSubmapPose (line 41) | transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
FILE: mapping_2d/sparse_pose_graph/constraint_builder.h
function namespace (line 44) | namespace cartographer {
FILE: mapping_2d/sparse_pose_graph/optimization_problem.cc
type cartographer (line 35) | namespace cartographer {
type mapping_2d (line 36) | namespace mapping_2d {
type sparse_pose_graph (line 37) | namespace sparse_pose_graph {
function FromPose (line 44) | std::array<double, 3> FromPose(const transform::Rigid2d& pose) {
function ToPose (line 50) | transform::Rigid2d ToPose(const std::array<double, 3>& values) {
FILE: mapping_2d/sparse_pose_graph/optimization_problem.h
function namespace (line 33) | namespace cartographer {
FILE: mapping_2d/sparse_pose_graph/spa_cost_function.h
function namespace (line 31) | namespace cartographer {
FILE: mapping_2d/sparse_pose_graph_test.cc
type cartographer (line 34) | namespace cartographer {
type mapping_2d (line 35) | namespace mapping_2d {
class SparsePoseGraphTest (line 38) | class SparsePoseGraphTest : public ::testing::Test {
method SparsePoseGraphTest (line 40) | SparsePoseGraphTest() : thread_pool_(1) {
method MoveRelativeWithNoise (line 146) | void MoveRelativeWithNoise(const transform::Rigid2d& movement,
method MoveRelative (line 169) | void MoveRelative(const transform::Rigid2d& movement) {
function TEST_F (line 180) | TEST_F(SparsePoseGraphTest, EmptyMap) {
function TEST_F (line 186) | TEST_F(SparsePoseGraphTest, NoMovement) {
function TEST_F (line 202) | TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
function TEST_F (line 220) | TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
function TEST_F (line 238) | TEST_F(SparsePoseGraphTest, OverlappingScans) {
FILE: mapping_2d/submaps.cc
type cartographer (line 31) | namespace cartographer {
type mapping_2d (line 32) | namespace mapping_2d {
function WriteDebugImage (line 36) | void WriteDebugImage(const string& filename,
function ProbabilityGrid (line 68) | ProbabilityGrid ComputeCroppedProbabilityGrid(
function CreateSubmapsOptions (line 88) | proto::SubmapsOptions CreateSubmapsOptions(
function Submap (line 130) | const Submap* Submaps::Get(int index) const {
FILE: mapping_2d/submaps.h
function namespace (line 35) | namespace cartographer {
FILE: mapping_2d/submaps_test.cc
type cartographer (line 28) | namespace cartographer {
type mapping_2d (line 29) | namespace mapping_2d {
function TEST (line 32) | TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
FILE: mapping_2d/xy_index.h
function namespace (line 31) | namespace cartographer {
FILE: mapping_2d/xy_index_test.cc
type cartographer (line 21) | namespace cartographer {
type mapping_2d (line 22) | namespace mapping_2d {
function TEST (line 25) | TEST(XYIndexTest, CellLimitsToProto) {
function TEST (line 32) | TEST(XYIndexTest, CellLimitsProtoConstructor) {
function TEST (line 42) | TEST(XYIndexTest, XYIndexRangeIterator) {
FILE: mapping_3d/acceleration_cost_function.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/ceres_pose.cc
type cartographer (line 19) | namespace cartographer {
type mapping_3d (line 20) | namespace mapping_3d {
FILE: mapping_3d/ceres_pose.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_3d/global_trajectory_builder.cc
type cartographer (line 21) | namespace cartographer {
type mapping_3d (line 22) | namespace mapping_3d {
FILE: mapping_3d/global_trajectory_builder.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_3d/hybrid_grid.h
function namespace (line 35) | namespace cartographer {
function Grow (line 385) | void Grow() {
function explicit (line 421) | explicit HybridGridBase(const float resolution) : resolution_(resolution...
function Eigen (line 436) | static Eigen::Array3i GetOctant(const int i) {
function Iterator (line 451) | Iterator end() const {
function SetProbability (line 484) | void SetProbability(const Eigen::Array3i& index, const float probability) {
function StartUpdate (line 489) | void StartUpdate() {
function ApplyLookupTable (line 504) | bool ApplyLookupTable(const Eigen::Array3i& index,
function GetProbability (line 518) | float GetProbability(const Eigen::Array3i& index) const {
function IsKnown (line 523) | bool IsKnown(const Eigen::Array3i& index) const { return value(index) !=...
function proto (line 530) | inline proto::HybridGrid ToProto(const HybridGrid& grid) {
FILE: mapping_3d/hybrid_grid_test.cc
type cartographer (line 25) | namespace cartographer {
type mapping_3d (line 26) | namespace mapping_3d {
function TEST (line 29) | TEST(HybridGridTest, ApplyOdds) {
function TEST (line 76) | TEST(HybridGridTest, GetProbability) {
function TEST (line 95) | TEST(HybridGridTest, GetCellIndex) {
function TEST (line 118) | TEST(HybridGridTest, GetCenterOfCell) {
class RandomHybridGridTest (line 129) | class RandomHybridGridTest : public ::testing::Test {
method RandomHybridGridTest (line 131) | RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
function TEST_F (line 157) | TEST_F(RandomHybridGridTest, TestIteration) {
function TEST_F (line 188) | TEST_F(RandomHybridGridTest, ToProto) {
type EigenComparator (line 214) | struct EigenComparator {
function TEST_F (line 223) | TEST_F(RandomHybridGridTest, FromProto) {
FILE: mapping_3d/imu_integration.cc
type cartographer (line 19) | namespace cartographer {
type mapping_3d (line 20) | namespace mapping_3d {
function IntegrateImu (line 22) | IntegrateImuResult<double> IntegrateImu(
FILE: mapping_3d/imu_integration.h
function namespace (line 30) | namespace mapping_3d {
FILE: mapping_3d/kalman_local_trajectory_builder.cc
type cartographer (line 28) | namespace cartographer {
type mapping_3d (line 29) | namespace mapping_3d {
FILE: mapping_3d/kalman_local_trajectory_builder.h
function namespace (line 34) | namespace cartographer {
FILE: mapping_3d/kalman_local_trajectory_builder_options.cc
type cartographer (line 22) | namespace cartographer {
type mapping_3d (line 23) | namespace mapping_3d {
function CreateKalmanLocalTrajectoryBuilderOptions (line 25) | proto::KalmanLocalTrajectoryBuilderOptions
FILE: mapping_3d/kalman_local_trajectory_builder_options.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/kalman_local_trajectory_builder_test.cc
type cartographer (line 34) | namespace cartographer {
type mapping_3d (line 35) | namespace mapping_3d {
class KalmanLocalTrajectoryBuilderTest (line 38) | class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
type TrajectoryNode (line 40) | struct TrajectoryNode {
method SetUp (line 45) | void SetUp() override { GenerateBubbles(); }
method CreateTrajectoryBuilderOptions (line 47) | proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOption...
method GenerateBubbles (line 132) | void GenerateBubbles() {
method CollideWithBox (line 147) | Eigen::Vector3f CollideWithBox(const Eigen::Vector3f& from,
method CollideWithBubbles (line 170) | Eigen::Vector3f CollideWithBubbles(const Eigen::Vector3f& from,
method GenerateRangeData (line 192) | sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) {
method AddLinearOnlyImuObservation (line 226) | void AddLinearOnlyImuObservation(const common::Time time,
method GenerateCorkscrewTrajectory (line 234) | std::vector<TrajectoryNode> GenerateCorkscrewTrajectory() {
method VerifyAccuracy (line 256) | void VerifyAccuracy(const std::vector<TrajectoryNode>& expected_tr...
function TEST_F (line 276) | TEST_F(KalmanLocalTrajectoryBuilderTest,
FILE: mapping_3d/local_trajectory_builder.cc
type cartographer (line 23) | namespace cartographer {
type mapping_3d (line 24) | namespace mapping_3d {
function CreateLocalTrajectoryBuilder (line 26) | std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajecto...
FILE: mapping_3d/local_trajectory_builder.h
function namespace (line 26) | namespace cartographer {
FILE: mapping_3d/local_trajectory_builder_interface.h
function namespace (line 29) | namespace cartographer {
FILE: mapping_3d/local_trajectory_builder_options.cc
type cartographer (line 27) | namespace cartographer {
type mapping_3d (line 28) | namespace mapping_3d {
function CreateLocalTrajectoryBuilderOptions (line 30) | proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOpt...
FILE: mapping_3d/local_trajectory_builder_options.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/motion_filter.cc
type cartographer (line 22) | namespace cartographer {
type mapping_3d (line 23) | namespace mapping_3d {
function CreateMotionFilterOptions (line 25) | proto::MotionFilterOptions CreateMotionFilterOptions(
FILE: mapping_3d/motion_filter.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_3d/motion_filter_test.cc
type cartographer (line 22) | namespace cartographer {
type mapping_3d (line 23) | namespace mapping_3d {
class MotionFilterTest (line 26) | class MotionFilterTest : public ::testing::Test {
method MotionFilterTest (line 28) | MotionFilterTest() {
method SecondsSinceEpoch (line 38) | common::Time SecondsSinceEpoch(int seconds) {
function TEST_F (line 45) | TEST_F(MotionFilterTest, NotInitialized) {
function TEST_F (line 51) | TEST_F(MotionFilterTest, NoChange) {
function TEST_F (line 59) | TEST_F(MotionFilterTest, TimeElapsed) {
function TEST_F (line 69) | TEST_F(MotionFilterTest, LinearMotion) {
function TEST_F (line 87) | TEST_F(MotionFilterTest, RotationalMotion) {
FILE: mapping_3d/optimizing_local_trajectory_builder.cc
type cartographer (line 32) | namespace cartographer {
type mapping_3d (line 33) | namespace mapping_3d {
class VelocityDeltaCostFunctor (line 39) | class VelocityDeltaCostFunctor {
method VelocityDeltaCostFunctor (line 41) | explicit VelocityDeltaCostFunctor(const double scaling_factor)
method VelocityDeltaCostFunctor (line 44) | VelocityDeltaCostFunctor(const VelocityDeltaCostFunctor&) = delete;
method VelocityDeltaCostFunctor (line 45) | VelocityDeltaCostFunctor& operator=(const VelocityDeltaCostFunctor...
class RelativeTranslationAndYawCostFunction (line 60) | class RelativeTranslationAndYawCostFunction {
method RelativeTranslationAndYawCostFunction (line 62) | RelativeTranslationAndYawCostFunction(const double translation_sca...
method RelativeTranslationAndYawCostFunction (line 69) | RelativeTranslationAndYawCostFunction(
method RelativeTranslationAndYawCostFunction (line 71) | RelativeTranslationAndYawCostFunction& operator=(
FILE: mapping_3d/optimizing_local_trajectory_builder.h
function namespace (line 35) | namespace cartographer {
FILE: mapping_3d/optimizing_local_trajectory_builder_options.cc
type cartographer (line 19) | namespace cartographer {
type mapping_3d (line 20) | namespace mapping_3d {
function CreateOptimizingLocalTrajectoryBuilderOptions (line 22) | proto::OptimizingLocalTrajectoryBuilderOptions
FILE: mapping_3d/optimizing_local_trajectory_builder_options.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/range_data_inserter.cc
type cartographer (line 23) | namespace cartographer {
type mapping_3d (line 24) | namespace mapping_3d {
function InsertMissesIntoGrid (line 28) | void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
function CreateRangeDataInserterOptions (line 57) | proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
FILE: mapping_3d/range_data_inserter.h
function namespace (line 25) | namespace cartographer {
FILE: mapping_3d/range_data_inserter_test.cc
type cartographer (line 25) | namespace cartographer {
type mapping_3d (line 26) | namespace mapping_3d {
class RangeDataInserterTest (line 29) | class RangeDataInserterTest : public ::testing::Test {
method RangeDataInserterTest (line 31) | RangeDataInserterTest() : hybrid_grid_(1.f) {
method InsertPointCloud (line 42) | void InsertPointCloud() {
method GetProbability (line 50) | float GetProbability(float x, float y, float z) const {
method IsKnown (line 55) | float IsKnown(float x, float y, float z) const {
function TEST_F (line 68) | TEST_F(RangeDataInserterTest, InsertPointCloud) {
function TEST_F (line 88) | TEST_F(RangeDataInserterTest, ProbabilityProgression) {
FILE: mapping_3d/rotation_cost_function.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/scan_matching/ceres_scan_matcher.cc
type cartographer (line 34) | namespace cartographer {
type mapping_3d (line 35) | namespace mapping_3d {
type scan_matching (line 36) | namespace scan_matching {
type YawOnlyQuaternionPlus (line 39) | struct YawOnlyQuaternionPlus {
function CreateCeresScanMatcherOptions (line 55) | proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
FILE: mapping_3d/scan_matching/ceres_scan_matcher.h
function namespace (line 30) | namespace cartographer {
FILE: mapping_3d/scan_matching/ceres_scan_matcher_test.cc
type cartographer (line 29) | namespace cartographer {
type mapping_3d (line 30) | namespace mapping_3d {
type scan_matching (line 31) | namespace scan_matching {
class CeresScanMatcherTest (line 34) | class CeresScanMatcherTest : public ::testing::Test {
method CeresScanMatcherTest (line 36) | CeresScanMatcherTest()
method TestFromInitialPose (line 66) | void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
function TEST_F (line 84) | TEST_F(CeresScanMatcherTest, PerfectEstimate) {
function TEST_F (line 89) | TEST_F(CeresScanMatcherTest, AlongX) {
function TEST_F (line 95) | TEST_F(CeresScanMatcherTest, AlongZ) {
function TEST_F (line 100) | TEST_F(CeresScanMatcherTest, AlongXYZ) {
function TEST_F (line 105) | TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
type cartographer (line 32) | namespace cartographer {
type mapping_3d (line 33) | namespace mapping_3d {
type scan_matching (line 34) | namespace scan_matching {
function CreateFastCorrelativeScanMatcherOptions (line 36) | proto::FastCorrelativeScanMatcherOptions
class PrecomputationGridStack (line 57) | class PrecomputationGridStack {
method PrecomputationGridStack (line 59) | PrecomputationGridStack(
method PrecomputationGrid (line 82) | const PrecomputationGrid& Get(int depth) const {
method max_depth (line 86) | int max_depth() const { return precomputation_grids_.size() - 1; }
function DiscreteScan (line 172) | DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
function Candidate (line 332) | Candidate FastCorrelativeScanMatcher::BranchAndBound(
FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher.h
function namespace (line 35) | namespace cartographer {
FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
type cartographer (line 30) | namespace cartographer {
type mapping_3d (line 31) | namespace mapping_3d {
type scan_matching (line 32) | namespace scan_matching {
function CreateFastCorrelativeScanMatcherTestOptions (line 35) | proto::FastCorrelativeScanMatcherOptions
function CreateRangeDataInserterTestOptions (line 54) | mapping_3d::proto::RangeDataInserterOptions
function TEST (line 65) | TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
FILE: mapping_3d/scan_matching/interpolated_grid.h
function namespace (line 24) | namespace cartographer {
FILE: mapping_3d/scan_matching/interpolated_grid_test.cc
type cartographer (line 23) | namespace cartographer {
type mapping_3d (line 24) | namespace mapping_3d {
type scan_matching (line 25) | namespace scan_matching {
class InterpolatedGridTest (line 28) | class InterpolatedGridTest : public ::testing::Test {
method InterpolatedGridTest (line 30) | InterpolatedGridTest()
method GetHybridGridProbability (line 41) | float GetHybridGridProbability(float x, float y, float z) const {
function TEST_F (line 50) | TEST_F(InterpolatedGridTest, InterpolatesGridPoints) {
function TEST_F (line 61) | TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
FILE: mapping_3d/scan_matching/occupied_space_cost_functor.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_3d/scan_matching/precomputation_grid.cc
type cartographer (line 26) | namespace cartographer {
type mapping_3d (line 27) | namespace mapping_3d {
type scan_matching (line 28) | namespace scan_matching {
function DivideByTwoRoundingTowardsNegativeInfinity (line 35) | inline int DivideByTwoRoundingTowardsNegativeInfinity(const int va...
function CellIndexAtHalfResolution (line 41) | Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cel...
function PrecomputationGrid (line 50) | PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& h...
function PrecomputationGrid (line 64) | PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
FILE: mapping_3d/scan_matching/precomputation_grid.h
function namespace (line 22) | namespace cartographer {
FILE: mapping_3d/scan_matching/precomputation_grid_test.cc
type cartographer (line 26) | namespace cartographer {
type mapping_3d (line 27) | namespace mapping_3d {
type scan_matching (line 28) | namespace scan_matching {
function TEST (line 31) | TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc
type cartographer (line 26) | namespace cartographer {
type mapping_3d (line 27) | namespace mapping_3d {
type scan_matching (line 28) | namespace scan_matching {
FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher.h
function namespace (line 29) | namespace cartographer {
FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc
type cartographer (line 31) | namespace cartographer {
type mapping_3d (line 32) | namespace mapping_3d {
type scan_matching (line 33) | namespace scan_matching {
class RealTimeCorrelativeScanMatcherTest (line 36) | class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
method RealTimeCorrelativeScanMatcherTest (line 38) | RealTimeCorrelativeScanMatcherTest()
method TestFromInitialPose (line 66) | void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
function TEST_F (line 82) | TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
function TEST_F (line 87) | TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
function TEST_F (line 92) | TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
function TEST_F (line 97) | TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
function TEST_F (line 102) | TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
function TEST_F (line 108) | TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
function TEST_F (line 114) | TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
FILE: mapping_3d/scan_matching/rotation_delta_cost_functor.h
function namespace (line 27) | namespace cartographer {
FILE: mapping_3d/scan_matching/rotational_scan_matcher.cc
type cartographer (line 24) | namespace cartographer {
type mapping_3d (line 25) | namespace mapping_3d {
type scan_matching (line 26) | namespace scan_matching {
function AddValueToHistogram (line 34) | void AddValueToHistogram(float angle, const float value,
function ComputeCentroid (line 51) | Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) {
type AngleValuePair (line 60) | struct AngleValuePair {
function AddPointCloudSliceToValueVector (line 65) | void AddPointCloudSliceToValueVector(
function SortSlice (line 98) | sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
function GetValuesForHistogram (line 125) | std::vector<AngleValuePair> GetValuesForHistogram(
function AddValuesToHistogram (line 138) | void AddValuesToHistogram(const std::vector<AngleValuePair>& value...
FILE: mapping_3d/scan_matching/rotational_scan_matcher.h
function namespace (line 26) | namespace cartographer {
FILE: mapping_3d/scan_matching/translation_delta_cost_functor.h
function namespace (line 23) | namespace cartographer {
FILE: mapping_3d/sparse_pose_graph.cc
type cartographer (line 38) | namespace cartographer {
type mapping_3d (line 39) | namespace mapping_3d {
FILE: mapping_3d/sparse_pose_graph.h
function namespace (line 42) | namespace cartographer {
FILE: mapping_3d/sparse_pose_graph/constraint_builder.cc
type cartographer (line 38) | namespace cartographer {
type mapping_3d (line 39) | namespace mapping_3d {
type sparse_pose_graph (line 40) | namespace sparse_pose_graph {
FILE: mapping_3d/sparse_pose_graph/constraint_builder.h
function namespace (line 44) | namespace cartographer {
FILE: mapping_3d/sparse_pose_graph/optimization_problem.cc
type cartographer (line 42) | namespace cartographer {
type mapping_3d (line 43) | namespace mapping_3d {
type sparse_pose_graph (line 44) | namespace sparse_pose_graph {
type ConstantYawQuaternionPlus (line 48) | struct ConstantYawQuaternionPlus {
FILE: mapping_3d/sparse_pose_graph/optimization_problem.h
function namespace (line 34) | namespace mapping_3d {
FILE: mapping_3d/sparse_pose_graph/optimization_problem_test.cc
type cartographer (line 29) | namespace cartographer {
type mapping_3d (line 30) | namespace mapping_3d {
type sparse_pose_graph (line 31) | namespace sparse_pose_graph {
class OptimizationProblemTest (line 34) | class OptimizationProblemTest : public ::testing::Test {
method OptimizationProblemTest (line 36) | OptimizationProblemTest()
method CreateOptions (line 40) | mapping::sparse_pose_graph::proto::OptimizationProblemOptions
method RandomTransform (line 60) | transform::Rigid3d RandomTransform(double translation_size,
method RandomYawOnlyTransform (line 77) | transform::Rigid3d RandomYawOnlyTransform(double translation_size,
function AddNoise (line 96) | transform::Rigid3d AddNoise(const transform::Rigid3d& transform,
function TEST_F (line 104) | TEST_F(OptimizationProblemTest, ReducesNoise) {
FILE: mapping_3d/sparse_pose_graph/spa_cost_function.h
function namespace (line 31) | namespace cartographer {
FILE: mapping_3d/submaps.cc
type cartographer (line 26) | namespace cartographer {
type mapping_3d (line 27) | namespace mapping_3d {
type RaySegment (line 33) | struct RaySegment {
function GenerateSegmentForSlice (line 41) | void GenerateSegmentForSlice(const sensor::RangeData& range_data,
function UpdateFreeSpaceFromSegment (line 103) | void UpdateFreeSpaceFromSegment(const RaySegment& segment,
function InsertSegmentsIntoProbabilityGrid (line 138) | void InsertSegmentsIntoProbabilityGrid(const std::vector<RaySegment>...
function FilterRangeDataByMaxRange (line 175) | sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData&...
function InsertIntoProbabilityGrid (line 188) | void InsertIntoProbabilityGrid(
function CreateSubmapsOptions (line 202) | proto::SubmapsOptions CreateSubmapsOptions(
function Submap (line 236) | const Submap* Submaps::Get(int index) const {
function string (line 367) | string Submaps::ComputePixelValues(
FILE: mapping_3d/submaps.h
function namespace (line 38) | namespace cartographer {
FILE: mapping_3d/translation_cost_function.h
function namespace (line 23) | namespace cartographer {
FILE: sensor/collator.cc
type cartographer (line 5) | namespace cartographer {
type sensor (line 6) | namespace sensor {
FILE: sensor/collator.h
function namespace (line 15) | namespace cartographer {
FILE: sensor/collator_test.cc
type cartographer (line 13) | namespace cartographer {
type sensor (line 14) | namespace sensor {
function TEST (line 17) | TEST(Collator, Ordering) {
FILE: sensor/compressed_point_cloud.cc
type cartographer (line 9) | namespace cartographer {
type sensor (line 10) | namespace sensor {
type RasterPoint (line 92) | struct RasterPoint {
function PointCloud (line 155) | PointCloud CompressedPointCloud::Decompress() const {
FILE: sensor/compressed_point_cloud.h
function namespace (line 13) | namespace cartographer {
FILE: sensor/compressed_point_cloud_test.cc
type Eigen (line 7) | namespace Eigen {
function PrintTo (line 12) | void PrintTo(const Vector3f& x, std::ostream* os) {
type cartographer (line 18) | namespace cartographer {
type sensor (line 19) | namespace sensor {
function TestPoint (line 36) | void TestPoint(const Eigen::Vector3f& p) {
function TEST (line 44) | TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
function TEST (line 58) | TEST(CompressPointCloudTest, Compresses) {
function TEST (line 74) | TEST(CompressPointCloudTest, CompressesEmptyPointCloud) {
function TEST (line 84) | TEST(CompressPointCloudTest, CompressesNoGaps) {
FILE: sensor/configuration.cc
type cartographer (line 12) | namespace cartographer {
type sensor (line 13) | namespace sensor {
function CreateSensorConfiguration (line 23) | proto::Configuration::Sensor CreateSensorConfiguration(
function CreateConfiguration (line 39) | proto::Configuration CreateConfiguration(
function IsEnabled (line 53) | bool IsEnabled(const string& frame_id,
function GetTransformToTracking (line 67) | transform::Rigid3d GetTransformToTracking(
FILE: sensor/configuration.h
function namespace (line 10) | namespace cartographer {
FILE: sensor/data.h
function namespace (line 10) | namespace cartographer {
FILE: sensor/ordered_multi_queue.cc
type cartographer (line 11) | namespace cartographer {
type sensor (line 12) | namespace sensor {
function QueueKey (line 77) | QueueKey OrderedMultiQueue::GetBlocker() const {
FILE: sensor/ordered_multi_queue.h
function namespace (line 17) | namespace cartographer {
FILE: sensor/ordered_multi_queue_le_test.cc
type cartographer (line 11) | namespace cartographer {
type sensor (line 12) | namespace sensor {
class OrderedMultiQueueTest (line 21) | class OrderedMultiQueueTest : public ::testing::Test {
method SetUp (line 29) | void SetUp() {
method MakeImu (line 43) | std::unique_ptr<Data> MakeImu(const int ordinal) {//时间,序列
function TEST_F (line 54) | TEST_F(OrderedMultiQueueTest, Ordering) {
function TEST_F (line 133) | TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) {
function TEST_F (line 163) | TEST_F(OrderedMultiQueueTest, CommonStartTimePerTrajectory) {
FILE: sensor/ordered_multi_queue_test.cc
type cartographer (line 9) | namespace cartographer {
type sensor (line 10) | namespace sensor {
class OrderedMultiQueueTest (line 19) | class OrderedMultiQueueTest : public ::testing::Test {
method SetUp (line 27) | void SetUp() {
method MakeImu (line 41) | std::unique_ptr<Data> MakeImu(const int ordinal) {//时间,序列
function TEST_F (line 52) | TEST_F(OrderedMultiQueueTest, Ordering) {
function TEST_F (line 91) | TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) {
function TEST_F (line 108) | TEST_F(OrderedMultiQueueTest, CommonStartTimePerTrajectory) {
FILE: sensor/point_cloud.cc
type cartographer (line 7) | namespace cartographer {
type sensor (line 8) | namespace sensor {
function PointCloud (line 16) | PointCloud TransformPointCloud(const PointCloud& point_cloud,
function PointCloud (line 31) | PointCloud Crop(const PointCloud& point_cloud, const float min_z,
function ToProto (line 43) | proto::PointCloud ToProto(const PointCloud& point_cloud) {
function PointCloud (line 54) | PointCloud ToPointCloud(const proto::PointCloud& proto) {
FILE: sensor/point_cloud.h
function namespace (line 27) | namespace cartographer {
FILE: sensor/point_cloud_test.cc
type cartographer (line 10) | namespace cartographer {
type sensor (line 11) | namespace sensor {
function TEST (line 14) | TEST(PointCloudTest, TransformPointCloud) {
FILE: sensor/range_data.cc
type cartographer (line 7) | namespace cartographer {
type sensor (line 8) | namespace sensor {
function ToProto (line 31) | proto::RangeData ToProto(const RangeData& range_data) {
function RangeData (line 39) | RangeData FromProto(const proto::RangeData& proto) {
function RangeData (line 51) | RangeData TransformRangeData(const RangeData& range_data,
function RangeData (line 63) | RangeData CropRangeData(const RangeData& range_data, const float min_z,
function CompressedRangeData (line 72) | CompressedRangeData Compress(const RangeData& range_data) {
function RangeData (line 81) | RangeData Decompress(const CompressedRangeData& compressed_range_dat...
FILE: sensor/range_data.h
function namespace (line 10) | namespace cartographer {
FILE: sensor/range_data_test.cc
type cartographer (line 9) | namespace cartographer {
type sensor (line 10) | namespace sensor {
function TEST (line 22) | TEST(RangeDataTest, Compression) {
FILE: sensor/voxel_filter.cc
type cartographer (line 8) | namespace cartographer {
type sensor (line 9) | namespace sensor {
function PointCloud (line 18) | PointCloud FilterByMaxRange(const PointCloud& point_cloud,
function PointCloud (line 44) | PointCloud AdaptivelyVoxelFiltered(
function PointCloud (line 110) | PointCloud VoxelFiltered(const PointCloud& point_cloud, const float ...
function PointCloud (line 135) | const PointCloud& VoxelFilter::point_cloud() const { return point_cl...
function CreateAdaptiveVoxelFilterOptions (line 155) | proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
function PointCloud (line 172) | PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud...
FILE: sensor/voxel_filter.h
function namespace (line 30) | namespace cartographer {
FILE: sensor/voxel_filter_test.cc
type cartographer (line 8) | namespace cartographer {
type sensor (line 9) | namespace sensor {
function TEST (line 14) | TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
FILE: transform/rigid_transform.cc
type cartographer (line 12) | namespace cartographer {
type transform (line 13) | namespace transform {
function TranslationFromDictionary (line 19) | Eigen::Vector3d TranslationFromDictionary(
function RollPitchYaw (line 37) | Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
function FromDictionary (line 59) | transform::Rigid3d FromDictionary(common::LuaParameterDictionary* di...
FILE: transform/rigid_transform.h
function namespace (line 14) | namespace cartographer {
FILE: transform/rigid_transform_le2_grid2_test.cc
type cartographer (line 12) | namespace cartographer {
type transform (line 13) | namespace transform {
function TEST (line 18) | TEST(Rigid2s, rigid_1) {
function TEST (line 37) | TEST(Rigid2s, rigid_2) {
function TEST (line 63) | TEST(Rigid2s, rigid_3) {
FILE: transform/rigid_transform_test_helpers.h
function namespace (line 14) | namespace cartographer {
FILE: transform/transform.cc
type cartographer (line 4) | namespace cartographer {
type transform (line 5) | namespace transform {
function Rigid2d (line 8) | Rigid2d ToRigid2(const proto::Rigid2d& pose) {
function ToEigen (line 13) | Eigen::Vector2d ToEigen(const proto::Vector2d& vector) {
function ToEigen (line 17) | Eigen::Vector3f ToEigen(const proto::Vector3f& vector) { //同上
function ToEigen (line 21) | Eigen::Vector3d ToEigen(const proto::Vector3d& vector) { //同上
function ToEigen (line 26) | Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion) {
function ToProto (line 31) | proto::Rigid2d ToProto(const transform::Rigid2d& transform) {
function ToProto (line 41) | proto::Rigid2f ToProto(const transform::Rigid2f& transform) {
function ToProto (line 49) | proto::Rigid3d ToProto(const transform::Rigid3d& rigid) {
function ToRigid3 (line 56) | transform::Rigid3d ToRigid3(const proto::Rigid3d& rigid) {
function ToProto (line 63) | proto::Rigid3f ToProto(const transform::Rigid3f& rigid) {
function ToProto (line 70) | proto::Vector2d ToProto(const Eigen::Vector2d& vector) {
function ToProto (line 77) | proto::Vector3f ToProto(const Eigen::Vector3f& vector) {
function ToProto (line 85) | proto::Vector3d ToProto(const Eigen::Vector3d& vector) {
function ToProto (line 93) | proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion) {
function ToProto (line 102) | proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion) {
FILE: transform/transform.h
function namespace (line 13) | namespace cartographer {
FILE: transform/transform_interpolation_buffer.cc
type cartographer (line 12) | namespace cartographer {
type transform (line 13) | namespace transform {
FILE: transform/transform_interpolation_buffer.h
function namespace (line 12) | namespace cartographer {
FILE: transform/transform_interpolation_buffer_test.cc
type cartographer (line 10) | namespace cartographer {
type transform (line 11) | namespace transform {
function TEST (line 14) | TEST(TransformInterpolationBufferTest, testHas) {
function TEST (line 31) | TEST(TransformInterpolationBufferTest, testLookup) {
FILE: transform/transform_test.cc
type cartographer (line 10) | namespace cartographer {
type transform (line 11) | namespace transform {
function TEST (line 14) | TEST(TransformTest, GetAngle) {
function TEST (line 37) | TEST(TransformTest, GetYaw) {
function TEST (line 44) | TEST(TransformTest, GetYawAxisOrdering) {
function TEST (line 53) | TEST(TransformTest, Embed3D) {
Condensed preview — 273 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,061K chars).
[
{
"path": ".gitignore",
"chars": 466,
"preview": "# Prerequisites\n*.d\n\n# Compiled Object files\n*.slo\n*.lo\n*.o\n*.obj\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Compiled Dynamic"
},
{
"path": "README.md",
"chars": 552,
"preview": "# cartographer\n关于谷歌slam地图库 [cartographer](https://github.com/googlecartographer/cartographer)的源码注释。(只含源码)\n\n \n\n博客文章：\n\n\n\n*"
},
{
"path": "common/blocking_queue.h",
"chars": 3541,
"preview": "\n#ifndef CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_\n#define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_\n\n#include <cstddef>\n#includ"
},
{
"path": "common/blocking_queue_test.cc",
"chars": 3714,
"preview": "\n#include \"cartographer/common/blocking_queue.h\"\n\n#include <memory>\n#include <thread>\n\n#include \"cartographer/common/mak"
},
{
"path": "common/ceres_solver_options.cc",
"chars": 1739,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/ceres_solver_options.h",
"chars": 1291,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/config.h",
"chars": 938,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/config.h.cmake",
"chars": 1000,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/configuration_file_resolver.cc",
"chars": 1980,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/configuration_file_resolver.h",
"chars": 1885,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/fixed_ratio_sampler.cc",
"chars": 659,
"preview": "\n\n#include \"cartographer/common/fixed_ratio_sampler.h\"\n\nnamespace cartographer {\nnamespace common {\n\nFixedRatioSampler::"
},
{
"path": "common/fixed_ratio_sampler.h",
"chars": 1213,
"preview": "\n#ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_\n#define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_\n\n#include <string"
},
{
"path": "common/fixed_ratio_sampler_test.cc",
"chars": 1158,
"preview": "\n\n#include \"cartographer/common/fixed_ratio_sampler.h\"\n\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamespace com"
},
{
"path": "common/histogram.cc",
"chars": 2300,
"preview": "\n\n#include \"cartographer/common/histogram.h\"\n\n#include <algorithm>\n#include <numeric>\n#include <string>\n\n#include \"carto"
},
{
"path": "common/histogram.h",
"chars": 1256,
"preview": "\n\n#ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_\n#define CARTOGRAPHER_COMMON_HISTOGRAM_H_\n\n#include <map>\n#include <string>\n#i"
},
{
"path": "common/lua.h",
"chars": 739,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/lua_parameter_dictionary.cc",
"chars": 15425,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/lua_parameter_dictionary.h",
"chars": 5546,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/lua_parameter_dictionary_test.cc",
"chars": 7298,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/lua_parameter_dictionary_test_helpers.h",
"chars": 1782,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "common/make_unique.h",
"chars": 1323,
"preview": "\n\n#ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_\n#define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_\n\n#include <cstddef>\n#include <me"
},
{
"path": "common/make_unique_test.cc",
"chars": 1063,
"preview": "\n#include <iostream>\n#include <string>\n#include \"cartographer/common/make_unique.h\"\n#include \"gtest/gtest.h\"\n//using nam"
},
{
"path": "common/math.h",
"chars": 1738,
"preview": "/*\ncommon/math.h文件主要实现数学计算，包括：\n区间截断.求n次方.求平方.幅度角度转换.归一化.反正切值\n*/\n\n#ifndef CARTOGRAPHER_COMMON_MATH_H_\n#define CARTOGRAPHE"
},
{
"path": "common/math_test.cc",
"chars": 1399,
"preview": "\n\n#include \"cartographer/common/math.h\"\n\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamespace common {\nnamespace"
},
{
"path": "common/mutex.h",
"chars": 3029,
"preview": "\n/*\n\ncommon/mutex.h主要是对c++11 的mutex的封装。\n\n*/\n#ifndef CARTOGRAPHER_COMMON_MUTEX_H_\n#define CARTOGRAPHER_COMMON_MUTEX_H_\n\n#"
},
{
"path": "common/port.h",
"chars": 2950,
"preview": "\n/*\n common-port.h文件主要实现2大功能：\n 1，使用std::lround对浮点数进行四舍五入取整运算\n 2，利用boost的iostreams/filter/gzip对字符串压缩与解压缩\n*/\n\n#ifndef C"
},
{
"path": "common/port_le_test.cpp",
"chars": 1596,
"preview": "#include <vector>\n#include <iostream>\n#include <vector> \n /*\n c++11 已经支持back_inserter。\n */\n\t//#include<boost/iostreams/"
},
{
"path": "common/proto/ceres_solver_options.proto",
"chars": 937,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "common/rate_timer.h",
"chars": 4018,
"preview": "\n#ifndef CARTOGRAPHER_COMMON_RATE_TIMER_H_\n#define CARTOGRAPHER_COMMON_RATE_TIMER_H_\n\n#include <chrono>\n#include <deque>"
},
{
"path": "common/rate_timer_test.cc",
"chars": 1702,
"preview": "\n#include \"cartographer/common/rate_timer.h\"\n\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamespace common {\nname"
},
{
"path": "common/thread_pool.cc",
"chars": 1929,
"preview": "\n\n#include \"cartographer/common/thread_pool.h\"\n\n#include <unistd.h>\n#include <algorithm>\n#include <chrono>\n#include <num"
},
{
"path": "common/thread_pool.h",
"chars": 1468,
"preview": "\n#ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_\n#define CARTOGRAPHER_COMMON_THREAD_POOL_H_\n\n#include <deque>\n#include <funct"
},
{
"path": "common/time.cc",
"chars": 1180,
"preview": "\n\n#include \"cartographer/common/time.h\"\n\n#include <string>\n\nnamespace cartographer {\nnamespace common {\n\n//duration_cast"
},
{
"path": "common/time.h",
"chars": 2570,
"preview": "\n /*\n预备知识:\nc++11 提供了语言级别的时间函数.包括duration和time_point\nduration是时间段,指的是某单位时间上的一个明确的tick(片刻数)：\n3分钟->\"3个1分钟\",\n1.5个\"1/3秒\" :1.5"
},
{
"path": "ground_truth/compute_relations_metrics_main.cc",
"chars": 5774,
"preview": "\n#include <algorithm>\n#include <cmath>\n#include <fstream>\n#include <iomanip>\n#include <iostream>\n#include <sstream>\n#inc"
},
{
"path": "io/cairo_types.h",
"chars": 1279,
"preview": "#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_\n#define CARTOGRAPHER_IO_CAIRO_TYPES_H_\n\n#include <memory>\n\n#include \"cairo/cairo."
},
{
"path": "io/coloring_points_processor.cc",
"chars": 2154,
"preview": "\n#include \"cartographer/io/coloring_points_processor.h\"\n\n#include \"Eigen/Core\"\n#include \"cartographer/common/make_unique"
},
{
"path": "io/coloring_points_processor.h",
"chars": 1688,
"preview": "\n#ifndef CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_\n\n#include <me"
},
{
"path": "io/counting_points_processor.cc",
"chars": 1301,
"preview": "\n#include \"cartographer/io/counting_points_processor.h\"\n\n#include \"cartographer/common/make_unique.h\"\n#include \"glog/log"
},
{
"path": "io/counting_points_processor.h",
"chars": 1266,
"preview": "\n#ifndef CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_\n\n#include \"ca"
},
{
"path": "io/file_writer.cc",
"chars": 963,
"preview": "\n#include \"cartographer/io/file_writer.h\"\n\nnamespace cartographer {\nnamespace io {\n\n//out:File open for writing: the int"
},
{
"path": "io/file_writer.h",
"chars": 1607,
"preview": "\n#ifndef CARTOGRAPHER_IO_FILE_WRITER_H_\n#define CARTOGRAPHER_IO_FILE_WRITER_H_\n\n#include <fstream>\n#include <functional>"
},
{
"path": "io/fixed_ratio_sampling_points_processor.cc",
"chars": 2332,
"preview": "\n\n#include \"cartographer/io/fixed_ratio_sampling_points_processor.h\"\n\n#include \"Eigen/Core\"\n#include \"cartographer/commo"
},
{
"path": "io/fixed_ratio_sampling_points_processor.h",
"chars": 1761,
"preview": "\n#ifndef CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PR"
},
{
"path": "io/intensity_to_color_points_processor.cc",
"chars": 1807,
"preview": "\n\n#include \"cartographer/io/intensity_to_color_points_processor.h\"\n\n#include \"Eigen/Core\"\n#include \"cartographer/common/"
},
{
"path": "io/intensity_to_color_points_processor.h",
"chars": 1920,
"preview": "\n#ifndef CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCES"
},
{
"path": "io/min_max_range_filtering_points_processor.cc",
"chars": 1668,
"preview": "\n#include \"cartographer/io/min_max_range_filtering_points_processor.h\"\n\n#include \"cartographer/common/lua_parameter_dict"
},
{
"path": "io/min_max_range_filtering_points_processor.h",
"chars": 1799,
"preview": "\n#ifndef CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POI"
},
{
"path": "io/null_points_processor.h",
"chars": 782,
"preview": "\n\n#ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_\n\n#include \"cartograp"
},
{
"path": "io/outlier_removing_points_processor.cc",
"chars": 3460,
"preview": "\n\n#include \"cartographer/io/outlier_removing_points_processor.h\"\n\n#include \"cartographer/common/lua_parameter_dictionary"
},
{
"path": "io/outlier_removing_points_processor.h",
"chars": 2762,
"preview": "\n\n#ifndef CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR"
},
{
"path": "io/pcd_writing_points_processor.cc",
"chars": 4252,
"preview": "\n#include \"cartographer/io/pcd_writing_points_processor.h\"\n\n#include <iomanip>\n#include <sstream>\n#include <string>\n\n#in"
},
{
"path": "io/pcd_writing_points_processor.h",
"chars": 1678,
"preview": "\n#include <fstream>\n\n#include \"cartographer/common/lua_parameter_dictionary.h\"\n#include \"cartographer/io/file_writer.h\"\n"
},
{
"path": "io/ply_writing_points_processor.cc",
"chars": 4190,
"preview": "\n#include \"cartographer/io/ply_writing_points_processor.h\"\n\n#include <iomanip>\n#include <sstream>\n#include <string>\n\n#in"
},
{
"path": "io/ply_writing_points_processor.h",
"chars": 1356,
"preview": "\n#include \"cartographer/common/lua_parameter_dictionary.h\"\n#include \"cartographer/io/file_writer.h\"\n#include \"cartograph"
},
{
"path": "io/points_batch.cc",
"chars": 1009,
"preview": "\n#include \"cartographer/io/points_batch.h\"\n\nnamespace cartographer {\nnamespace io {\n\n/*\n先对to_remove做降序排列,然后按照to_remove依次"
},
{
"path": "io/points_batch.h",
"chars": 1986,
"preview": "\n#ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_\n#define CARTOGRAPHER_IO_POINTS_BATCH_H_\n\n#include <array>\n#include <cstdint>\n#i"
},
{
"path": "io/points_processor.h",
"chars": 1187,
"preview": "\n#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_\n\n#include <memory>\n\n#include \"c"
},
{
"path": "io/points_processor_pipeline_builder.cc",
"chars": 6526,
"preview": "\n\n#include \"cartographer/io/points_processor_pipeline_builder.h\"\n\n#include \"cartographer/common/make_unique.h\"\n#include "
},
{
"path": "io/points_processor_pipeline_builder.h",
"chars": 2293,
"preview": "\n\n#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_\n#define CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER"
},
{
"path": "io/proto_stream.cc",
"chars": 1847,
"preview": "\n#include \"cartographer/io/proto_stream.h\"\n\nnamespace cartographer {\nnamespace io {\n\nnamespace {\n\n// First eight bytes t"
},
{
"path": "io/proto_stream.h",
"chars": 1923,
"preview": "\n#ifndef CARTOGRAPHER_IO_PROTO_STREAM_H_\n#define CARTOGRAPHER_IO_PROTO_STREAM_H_\n\n#include <fstream>\n\n#include \"cartogra"
},
{
"path": "io/proto_stream_test.cc",
"chars": 1491,
"preview": "\n#include \"cartographer/io/proto_stream.h\"\n\n#include <errno.h>\n#include <stdio.h>\n#include <stdlib.h>\n#include <string.h"
},
{
"path": "io/xray_points_processor.cc",
"chars": 9179,
"preview": "\n#include \"cartographer/io/xray_points_processor.h\"\n\n#include <cmath>\n#include <string>\n\n#include \"Eigen/Core\"\n#include "
},
{
"path": "io/xray_points_processor.h",
"chars": 2972,
"preview": "#ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_\n\n#include <map>\n\n#incl"
},
{
"path": "io/xyz_writing_points_processor.cc",
"chars": 1891,
"preview": "#include \"cartographer/io/xyz_writing_points_processor.h\"\n\n#include <iomanip>\n\n#include \"cartographer/common/make_unique"
},
{
"path": "io/xyz_writing_points_processor.h",
"chars": 1345,
"preview": "\n#ifndef CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_\n#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_\n\n#inclu"
},
{
"path": "kalman_filter/gaussian_distribution.h",
"chars": 1789,
"preview": "\n#ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_\n#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_"
},
{
"path": "kalman_filter/gaussian_distribution_test.cc",
"chars": 677,
"preview": "#include \"cartographer/kalman_filter/gaussian_distribution.h\"\n\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamesp"
},
{
"path": "kalman_filter/pose_tracker.cc",
"chars": 12943,
"preview": "\n#include \"cartographer/kalman_filter/pose_tracker.h\"\n\n#include <cmath>\n#include <limits>\n#include <utility>\n\n#include \""
},
{
"path": "kalman_filter/pose_tracker.h",
"chars": 4987,
"preview": "\n#ifndef CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_\n#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_\n\n#include <deque>"
},
{
"path": "kalman_filter/pose_tracker_test.cc",
"chars": 8601,
"preview": "\n#include \"cartographer/kalman_filter/pose_tracker.h\"\n\n#include <random>\n\n#include \"cartographer/common/lua_parameter_di"
},
{
"path": "kalman_filter/proto/pose_tracker_options.proto",
"chars": 1238,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "kalman_filter/unscented_kalman_filter.h",
"chars": 12606,
"preview": "\n#ifndef CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_\n#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTE"
},
{
"path": "kalman_filter/unscented_kalman_filter_test.cc",
"chars": 2954,
"preview": "\n#include \"cartographer/kalman_filter/unscented_kalman_filter.h\"\n\n#include \"cartographer/kalman_filter/gaussian_distribu"
},
{
"path": "mapping/collated_trajectory_builder.cc",
"chars": 3054,
"preview": "\n#include \"cartographer/mapping/collated_trajectory_builder.h\"\n\n#include \"cartographer/common/time.h\"\n#include \"glog/log"
},
{
"path": "mapping/collated_trajectory_builder.h",
"chars": 2625,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_\n#define CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_"
},
{
"path": "mapping/detect_floors.cc",
"chars": 6577,
"preview": "\n#include \"cartographer/mapping/detect_floors.h\"\n\n#include <algorithm>\n#include <fstream>\n#include <vector>\n\n#include \"E"
},
{
"path": "mapping/detect_floors.h",
"chars": 1176,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_\n#define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_\n\n#include \"cartographer/map"
},
{
"path": "mapping/global_trajectory_builder_interface.h",
"chars": 1894,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_\n#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUIL"
},
{
"path": "mapping/id.h",
"chars": 3159,
"preview": "#ifndef CARTOGRAPHER_MAPPING_ID_H_\n#define CARTOGRAPHER_MAPPING_ID_H_\n\n#include <algorithm>\n#include <ostream>\n#include "
},
{
"path": "mapping/imu_tracker.cc",
"chars": 2715,
"preview": "\n#include \"cartographer/mapping/imu_tracker.h\"\n\n#include <cmath>\n#include <limits>\n\n#include \"cartographer/common/math.h"
},
{
"path": "mapping/imu_tracker.h",
"chars": 2909,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_\n#define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_\n\n#include \"Eigen/Geometry\"\n#inc"
},
{
"path": "mapping/map_builder.cc",
"chars": 6518,
"preview": "\n#include \"cartographer/mapping/map_builder.h\"\n\n#include <cmath>\n#include <limits>\n#include <memory>\n#include <unordered"
},
{
"path": "mapping/map_builder.h",
"chars": 3525,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_\n#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_\n\n#include <memory>\n#include <st"
},
{
"path": "mapping/odometry_state_tracker.cc",
"chars": 1148,
"preview": "\n#include \"cartographer/mapping/odometry_state_tracker.h\"\n\n#include \"cartographer/transform/rigid_transform.h\"\n\nnamespac"
},
{
"path": "mapping/odometry_state_tracker.h",
"chars": 1865,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_\n#define CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_\n\n#include"
},
{
"path": "mapping/probability_values.cc",
"chars": 2170,
"preview": "\n#include \"cartographer/mapping/probability_values.h\"\n\nnamespace cartographer {\nnamespace mapping {\n\nnamespace {\n\n\n//直接使"
},
{
"path": "mapping/probability_values.h",
"chars": 2327,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_\n#define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_\n\n#include <cmath>"
},
{
"path": "mapping/probability_values_test.cc",
"chars": 507,
"preview": "\n#include \"cartographer/mapping/probability_values.h\"\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamespace mappi"
},
{
"path": "mapping/proto/map_builder_options.proto",
"chars": 1024,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/sparse_pose_graph.proto",
"chars": 1942,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/sparse_pose_graph_options.proto",
"chars": 1948,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/submap_visualization.proto",
"chars": 1998,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/trajectory.proto",
"chars": 1168,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/trajectory_builder_options.proto",
"chars": 1054,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/proto/trajectory_connectivity.proto",
"chars": 1051,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/sparse_pose_graph/constraint_builder.cc",
"chars": 4118,
"preview": "\n#include \"cartographer/mapping/sparse_pose_graph/constraint_builder.h\"\n\n#include \"cartographer/mapping_2d/scan_matching"
},
{
"path": "mapping/sparse_pose_graph/constraint_builder.h",
"chars": 677,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_\n#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CON"
},
{
"path": "mapping/sparse_pose_graph/optimization_problem_options.cc",
"chars": 1749,
"preview": "\n#include \"cartographer/mapping/sparse_pose_graph/optimization_problem_options.h\"\n\n#include \"cartographer/common/ceres_s"
},
{
"path": "mapping/sparse_pose_graph/optimization_problem_options.h",
"chars": 1123,
"preview": "\n\n#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_\n#define CARTOGRAPHER_MAPPING_SPARSE_POS"
},
{
"path": "mapping/sparse_pose_graph/proto/constraint_builder_options.proto",
"chars": 2851,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/sparse_pose_graph/proto/optimization_problem_options.proto",
"chars": 1469,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping/sparse_pose_graph.cc",
"chars": 3652,
"preview": "\n#include \"cartographer/mapping/sparse_pose_graph.h\"\n\n#include \"cartographer/mapping/sparse_pose_graph/constraint_builde"
},
{
"path": "mapping/sparse_pose_graph.h",
"chars": 2936,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_\n#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_\n\n#include <set>\n#in"
},
{
"path": "mapping/submaps.cc",
"chars": 2609,
"preview": "\n#include \"cartographer/mapping/submaps.h\"\n\n#include <vector>\n\n#include \"cartographer/common/port.h\"\n#include \"cartograp"
},
{
"path": "mapping/submaps.h",
"chars": 4737,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_\n#define CARTOGRAPHER_MAPPING_SUBMAPS_H_\n\n#include <memory>\n#include <vector>\n\n#"
},
{
"path": "mapping/submaps_test.cc",
"chars": 723,
"preview": "\n#include \"cartographer/mapping/submaps.h\"\n\n#include <cmath>\n\n#include \"gtest/gtest.h\"\n\nnamespace cartographer {\nnamespa"
},
{
"path": "mapping/trajectory_builder.cc",
"chars": 893,
"preview": "\n#include \"cartographer/mapping/trajectory_builder.h\"\n\n#include \"cartographer/mapping_2d/local_trajectory_builder_option"
},
{
"path": "mapping/trajectory_builder.h",
"chars": 3472,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_\n#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_\n\n#include <functi"
},
{
"path": "mapping/trajectory_connectivity.cc",
"chars": 4046,
"preview": "\n#include \"cartographer/mapping/trajectory_connectivity.h\"\n\n#include <algorithm>\n#include <unordered_set>\n\n#include \"car"
},
{
"path": "mapping/trajectory_connectivity.h",
"chars": 3492,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_\n#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_\n\n#inclu"
},
{
"path": "mapping/trajectory_connectivity_test.cc",
"chars": 3405,
"preview": "\n#include \"cartographer/mapping/trajectory_connectivity.h\"\n\n#include <algorithm>\n#include <memory>\n#include <vector>\n\n#i"
},
{
"path": "mapping/trajectory_node.h",
"chars": 1524,
"preview": "\n#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_\n#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_\n\n#include <deque>\n#incl"
},
{
"path": "mapping_2d/global_trajectory_builder.cc",
"chars": 2722,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/global_trajectory_builder.h",
"chars": 2363,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/local_trajectory_builder.cc",
"chars": 10672,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/local_trajectory_builder.h",
"chars": 4547,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/local_trajectory_builder_options.cc",
"chars": 3124,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/local_trajectory_builder_options.h",
"chars": 1196,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/map_limits.h",
"chars": 3754,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/map_limits_test.cc",
"chars": 2179,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/probability_grid.h",
"chars": 8223,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/probability_grid_test.cc",
"chars": 7438,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/proto/cell_limits.proto",
"chars": 754,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/proto/local_trajectory_builder_options.proto",
"chars": 2874,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/proto/map_limits.proto",
"chars": 930,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/proto/probability_grid.proto",
"chars": 1046,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/proto/range_data_inserter_options.proto",
"chars": 1132,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/proto/submaps_options.proto",
"chars": 1342,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/range_data_inserter.cc",
"chars": 2709,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/range_data_inserter.h",
"chars": 2092,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/range_data_inserter_test.cc",
"chars": 4348,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/ray_casting.cc",
"chars": 6164,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/ray_casting.h",
"chars": 1480,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/ceres_scan_matcher.cc",
"chars": 4352,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/ceres_scan_matcher.h",
"chars": 2332,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/ceres_scan_matcher_test.cc",
"chars": 3491,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/correlative_scan_matcher.cc",
"chars": 5584,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/correlative_scan_matcher.h",
"chars": 3738,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/correlative_scan_matcher_test.cc",
"chars": 4092,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/fast_correlative_scan_matcher.cc",
"chars": 16260,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/fast_correlative_scan_matcher.h",
"chars": 6614,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc",
"chars": 9252,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/fast_global_localizer.cc",
"chars": 2102,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/fast_global_localizer.h",
"chars": 2022,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/occupied_space_cost_functor.h",
"chars": 4353,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto",
"chars": 1163,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto",
"chars": 1093,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto",
"chars": 1162,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc",
"chars": 6499,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/real_time_correlative_scan_matcher.h",
"chars": 3942,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc",
"chars": 4864,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/rotation_delta_cost_functor.h",
"chars": 1885,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/scan_matching/translation_delta_cost_functor.h",
"chars": 2126,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph/constraint_builder.cc",
"chars": 11268,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph/constraint_builder.h",
"chars": 7968,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph/optimization_problem.cc",
"chars": 8239,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph/optimization_problem.h",
"chars": 2993,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph/spa_cost_function.h",
"chars": 3158,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph.cc",
"chars": 19035,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph.h",
"chars": 8763,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/sparse_pose_graph_test.cc",
"chars": 10643,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/submaps.cc",
"chars": 6482,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/submaps.h",
"chars": 2584,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/submaps_test.cc",
"chars": 2156,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/xy_index.h",
"chars": 3567,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_2d/xy_index_test.cc",
"chars": 1902,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/acceleration_cost_function.h",
"chars": 3348,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/ceres_pose.cc",
"chars": 1752,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/ceres_pose.h",
"chars": 1638,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/global_trajectory_builder.cc",
"chars": 2677,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/global_trajectory_builder.h",
"chars": 2362,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/hybrid_grid.h",
"chars": 18329,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/hybrid_grid_test.cc",
"chars": 8786,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/imu_integration.cc",
"chars": 1146,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/imu_integration.h",
"chars": 3185,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/kalman_local_trajectory_builder.cc",
"chars": 10215,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/kalman_local_trajectory_builder.h",
"chars": 3573,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/kalman_local_trajectory_builder_options.cc",
"chars": 2057,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/kalman_local_trajectory_builder_options.h",
"chars": 1236,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/kalman_local_trajectory_builder_test.cc",
"chars": 10729,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/local_trajectory_builder.cc",
"chars": 1546,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/local_trajectory_builder.h",
"chars": 1260,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/local_trajectory_builder_interface.h",
"chars": 2458,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/local_trajectory_builder_options.cc",
"chars": 3403,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/local_trajectory_builder_options.h",
"chars": 1196,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/motion_filter.cc",
"chars": 2104,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/motion_filter.h",
"chars": 1791,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/motion_filter_test.cc",
"chars": 4288,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/optimizing_local_trajectory_builder.cc",
"chars": 19197,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/optimizing_local_trajectory_builder.h",
"chars": 4894,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/optimizing_local_trajectory_builder_options.cc",
"chars": 1760,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/optimizing_local_trajectory_builder_options.h",
"chars": 1260,
"preview": "/*\n * Copyright 2016 The Cartographer Authors\n *\n * Licensed under the Apache License, Version 2.0 (the \"License\");\n * y"
},
{
"path": "mapping_3d/proto/hybrid_grid.proto",
"chars": 1070,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_3d/proto/kalman_local_trajectory_builder_options.proto",
"chars": 1445,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_3d/proto/local_trajectory_builder_options.proto",
"chars": 2402,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_3d/proto/motion_filter_options.proto",
"chars": 1038,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_3d/proto/optimizing_local_trajectory_builder_options.proto",
"chars": 1035,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
},
{
"path": "mapping_3d/proto/range_data_inserter_options.proto",
"chars": 1144,
"preview": "// Copyright 2016 The Cartographer Authors\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you "
}
]
// ... and 73 more files (download for full content)
About this extraction
This page contains the full source code of the slam-code/cartographer GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 273 files (954.9 KB), approximately 253.7k tokens, and a symbol index with 824 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.