基于Cartographer 1.0.0的非ROS建图程序
下面是一个使用Cartographer 1.0.0库，不依赖ROS，直接使用激光雷达和IMU数据进行建图的C++程序示例。程序完成后会将地图保存为PGM和YAML文件。
程序代码
#include <iostream>
#include <fstream>
#include <memory>
#include <vector>
#include <string>#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/image.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"// 假设的传感器数据读取类
class SensorDataReader {
public:struct ImuData {double timestamp;double linear_acceleration_x;double linear_acceleration_y;double linear_acceleration_z;double angular_velocity_x;double angular_velocity_y;double angular_velocity_z;};struct LaserScan {double timestamp;std::vector<float> ranges;float angle_min;float angle_max;float angle_increment;float range_min;float range_max;};// 模拟从文件读取IMU数据static std::vector<ImuData> ReadImuData(const std::string& filename) {std::vector<ImuData> imu_data;// 这里应该是实际的文件读取代码// 示例中我们返回一些模拟数据for (int i = 0; i < 1000; ++i) {ImuData data;data.timestamp = i * 0.01;data.linear_acceleration_x = 0.0;data.linear_acceleration_y = 0.0;data.linear_acceleration_z = 9.8;data.angular_velocity_x = 0.0;data.angular_velocity_y = 0.0;data.angular_velocity_z = 0.0;imu_data.push_back(data);}return imu_data;}// 模拟从文件读取激光雷达数据static std::vector<LaserScan> ReadLaserData(const std::string& filename) {std::vector<LaserScan> laser_data;// 这里应该是实际的文件读取代码// 示例中我们返回一些模拟数据for (int i = 0; i < 100; ++i) {LaserScan scan;scan.timestamp = i * 0.1;scan.angle_min = -M_PI / 2;scan.angle_max = M_PI / 2;scan.angle_increment = M_PI / 180;scan.range_min = 0.1;scan.range_max = 10.0;int num_readings = (scan.angle_max - scan.angle_min) / scan.angle_increment;for (int j = 0; j < num_readings; ++j) {scan.ranges.push_back(5.0 + 0.1 * sin(j * 0.1 + i * 0.2));}laser_data.push_back(scan);}return laser_data;}
};// 地图保存功能
class MapSaver {
public:static void SaveMap(const cartographer::mapping::MapBuilder& map_builder,const std::string& map_filename) {auto trajectory_nodes = map_builder.GetTrajectoryNodePoses();auto submap_poses = map_builder.GetAllSubmapPoses();// 获取所有子图数据std::map<cartographer::mapping::SubmapId, cartographer::io::SubmapSlice> submap_slices;for (const auto& submap_id_pose : submap_poses) {const cartographer::mapping::SubmapId submap_id = submap_id_pose.id;auto submap_data = map_builder.GetSubmapData(submap_id);cartographer::io::SubmapSlice slice;slice.pose = submap_id_pose.data.pose;slice.metadata = submap_data.submap->ToProto();slice.submap_textures = cartographer::io::PaintSubmapSlices({{submap_id, submap_data}}, cartographer::transform::Rigid3d::Identity());submap_slices[submap_id] = slice;}// 绘制地图auto painted_slices = cartographer::io::PaintSubmapSlices(submap_slices, 0.05);// 保存PGM文件std::string pgm_filename = map_filename + ".pgm";cartographer::io::StreamFileWriter pgm_writer(pgm_filename);cartographer::io::Image image(painted_slices.surface, painted_slices.surface->width(),painted_slices.surface->height());cartographer::io::WritePgm(image, 255, pgm_writer);// 保存YAML文件std::string yaml_filename = map_filename + ".yaml";std::ofstream yaml_file(yaml_filename);yaml_file << "image: " << pgm_filename << "\n";yaml_file << "resolution: " << 0.05 << "\n";yaml_file << "origin: [" << painted_slices.origin.x() << ", " << painted_slices.origin.y() << ", 0.0]\n";yaml_file << "negate: 0\n";yaml_file << "occupied_thresh: 0.65\n";yaml_file << "free_thresh: 0.196\n";}
};int main(int argc, char** argv) {// 1. 加载配置文件std::string configuration_directory = ".";std::string configuration_basename = "my_cartographer_config.lua";auto file_resolver = std::make_unique<cartographer::common::ConfigurationFileResolver>(std::vector<std::string>{configuration_directory});std::string lua_code = file_resolver->GetFileContentOrDie(configuration_basename);auto lua_parameter_dictionary = cartographer::common::LuaParameterDictionary::NonReferenceCounted(lua_code, std::move(file_resolver));// 2. 创建MapBuilderauto map_builder_options = cartographer::mapping::CreateMapBuilderOptions(lua_parameter_dictionary->GetDictionary("map_builder").get());auto map_builder = std::make_unique<cartographer::mapping::MapBuilder>(map_builder_options);// 3. 创建轨迹const int trajectory_id = map_builder->AddTrajectoryBuilder({cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds()});auto* trajectory_builder = map_builder->GetTrajectoryBuilder(trajectory_id);// 4. 读取传感器数据auto imu_data = SensorDataReader::ReadImuData("imu_data.txt");auto laser_data = SensorDataReader::ReadLaserData("laser_data.txt");// 5. 处理IMU数据for (const auto& imu : imu_data) {cartographer::sensor::ImuData carto_imu;carto_imu.time = cartographer::common::FromUniversal(0) + cartographer::common::FromSeconds(imu.timestamp);carto_imu.linear_acceleration = Eigen::Vector3d(imu.linear_acceleration_x,imu.linear_acceleration_y,imu.linear_acceleration_z);carto_imu.angular_velocity = Eigen::Vector3d(imu.angular_velocity_x,imu.angular_velocity_y,imu.angular_velocity_z);trajectory_builder->AddImuData(carto_imu);}// 6. 处理激光雷达数据for (const auto& scan : laser_data) {cartographer::sensor::TimedPointCloud point_cloud;float angle = scan.angle_min;for (float range : scan.ranges) {if (range >= scan.range_min && range <= scan.range_max) {point_cloud.emplace_back(range * std::cos(angle),range * std::sin(angle),0.0, // z坐标0.0); // time}angle += scan.angle_increment;}auto range_data = cartographer::sensor::RangeData{Eigen::Vector3f::Zero(), // 原点cartographer::sensor::PointCloud(point_cloud),{}};trajectory_builder->AddRangefinderData(cartographer::common::FromUniversal(0) + cartographer::common::FromSeconds(scan.timestamp),cartographer::sensor::TimedRangeData{cartographer::transform::Rigid3d::Identity(),range_data});}// 7. 完成建图并保存map_builder->FinishTrajectory(trajectory_id);// 保存地图MapSaver::SaveMap(*map_builder, "output_map");std::cout << "建图完成，地图已保存为output_map.pgm和output_map.yaml" << std::endl;return 0;
}配置文件示例 (my_cartographer_config.lua)
include "map_builder.lua"
include "trajectory_builder.lua"options = {map_builder = MAP_BUILDER,trajectory_builder = TRAJECTORY_BUILDER,map_frame = "map",tracking_frame = "imu_link",published_frame = "base_link",odom_frame = "odom",provide_odom_frame = true,publish_frame_projected_to_2d = false,use_odometry = false,use_nav_sat = false,use_landmarks = false,num_laser_scans = 1,num_multi_echo_laser_scans = 0,num_subdivisions_per_laser_scan = 1,num_point_clouds = 0,lookup_transform_timeout_sec = 0.2,submap_publish_period_sec = 0.3,pose_publish_period_sec = 5e-3,trajectory_publish_period_sec = 30e-3,rangefinder_sampling_ratio = 1.,odometry_sampling_ratio = 1.,fixed_frame_pose_sampling_ratio = 1.,imu_sampling_ratio = 1.,landmarks_sampling_ratio = 1.,
}MAP_BUILDER.use_trajectory_builder_2d = trueTRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 10.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type = "PROBABILITY_GRID"
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05return options
编译说明
确保已安装Cartographer 1.0.0库及其依赖
创建一个CMake项目，链接Cartographer库
示例CMakeLists.txt:
cmakeCopy Code
cmake_minimum_required(VERSION 3.5)
project(cartographer_non_ros_example)
find_package(Cartographer REQUIRED)
add_executable(cartographer_non_ros_example main.cpp)
target_link_libraries(cartographer_non_ros_example
PRIVATE
Cartographer::cartographer
)
程序说明
程序首先加载Cartographer的Lua配置文件
创建MapBuilder和TrajectoryBuilder
从文件读取模拟的IMU和激光雷达数据
将传感器数据添加到Cartographer中进行处理
建图完成后，将地图保存为PGM和YAML格式
注意事项
实际使用时需要替换SensorDataReader类中的模拟数据读取为真实的传感器数据读取
根据实际传感器配置调整Lua配置文件参数
确保Cartographer库正确安装并配置
此示例假设激光雷达是2D的，如果是3D激光雷达需要调整配置和数据处理部分
这个程序提供了一个不使用ROS的Cartographer建图框架，你可以根据实际需求进行修改和扩展。
