基于cartographer 1.0.0 不使用ros 使用激光雷达数据和IMU数据融合实现的建图

基于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. 创建MapBuilder
    auto 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 = true

TRAJECTORY_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.05

return 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建图框架，你可以根据实际需求进行修改和扩展。