cartographer脱离ros得到全局图

阿里云2000元红包!本站用户参与享受九折优惠!

说下大体流程吧,开始想利用cartographer_ros 一点点去掉,然而ros的框架太庞大了,接下来我就仿照cartographer_ros写,数据的传输完全和cartographer一摸一样,缺啥就补啥,一般需要的ros中的东西就是数据类型的头文件,tf转换器
时间类型。这些小东西完全可以自己写。
中间遇到了一些坑总结下
1,有很多数据的数据类型是一样的,自己写的时候不要传错了,因为自己写在一个main中,很容易弄混,这个错了很难找的额
2,cartographer解析数据的速度太慢,找了半天,通过日志和时间戳一点点找,最后找到了个ScoreCandidate方法,然后在里面定位可能耗时间长的,如果没有数学公式的话,耗时间长一定是for循环次数太多咯,但是打印日志发现我的项目和cartographer_ros项目的for循环次数是一样的,里面牵扯到的vector长度也是一样(我开始就怕因为是自己写的,所以和cartographer_ros哪块不一样导致一些参数不一样以致于影响到后面)。也没有什么数学公式和迭代之类的。所以只能找其他原因了,之前依稀记得cmake里面提醒过一句,debug模式可能会执行得比较慢,因为算法匹配用到了gpu貌似,然后改成release模式,果然是正常速度了
3,找到submap数据,开始想通过topic取找,找到的是submaplist数据型类型,根本不是submap图像数据。后面又找了个电云数据。无奈只能去server里面找,找到了submapQuery。这个就找对了,然后找到相关类,然后打印,期间遇到protobuf这个东西,cartographer中很多数据都有个protobuf模式的,protobuf是个google的弄的,与平台无关,解析速度很快的数据类型。反正submap可protobuf.submap可以互相转,直接取数据取不到,反正要通过protobuf,这耽误了点时间去了解protobuf。submap数据是一维的,但它和二维数据的转换就是从左到右,从上到下这样的,python里面的numpy可以转,转完后用opencv显示
4,得到全局图很简单,思路有2个,第一种是按照数据处理流程找到全局图相关类,然后看这个全局图数据怎么对外提供,这个很累,试了一下没弄了。第二个是看官网利用cartographer_ros得到全局图,用到了writestate服务,和一个pb_to_ros_map之类的launch,然后去cartographer_ros中拷贝,然后放到自己代码中,一切搞定了
5,实时建图,待完善
6,在这个工作过程中就是各种定位问题拉,就像二分查找法一样。

#include "cartographer/ydli/CYdLidar.h"
#include <vector>
#include <iostream>
#include <string>
#include <signal.h>
#include <thread>
#include <mapping/2d/submap_2d.h>
//#include "cartographer/sensor/timed_point_cloud_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/cartographer_ros/node_options.h"
#include "cartographer/cartographer_ros/sensor_msg.h"
#include "cartographer/cartographer_ros/trajectory_options.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/transform/transform.h"
#include "cartographer/common/mutex.h"
#include "cartographer/ros/ros_time.h"
#include "cartographer/cartographer_ros/SubmapEntry.h"
#include "cartographer/common/time.h"
#include "cartographer/transform/transform.h"
#include <iostream>
#include <fstream>
#include<sstream>
//获得全局图
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/cartographer_ros/ros_map.h"
namespace cartographer {
    namespace src {
        const ::std::string kMessage = R"(
    expected_sensor_ids {
      id: "range_sensor"
      type: "range"
    }
    expected_sensor_ids {
      id: "imu_sensor"
      type: "imu"
    }
    trajectory_builder_options {
      trajectory_builder_2d_options {
        min_range: 20
        max_range: 30
      }
      pure_localization: true
      initial_trajectory_pose {
        relative_pose {
          translation {
            x: 1 y: 2 z: 3
          }
          rotation {
            w: 4 x: 5 y: 6 z: 7
          }
        }
        to_trajectory_id: 8
        timestamp: 9
      }
    }
  )";
        struct TrajectoryState {
            // Contains the trajectory state data received from local SLAM, after
            // it had processed accumulated 'range_data_in_local' and estimated
            // current 'local_pose' at 'time'.
            struct LocalSlamData {
                ::cartographer::common::Time time;
                ::cartographer::transform::Rigid3d local_pose;
                ::cartographer::sensor::RangeData range_data_in_local;
            };
            ::std::shared_ptr<const LocalSlamData> local_slam_data;
            ::cartographer::transform::Rigid3d local_to_map;
            ::std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
            cartographer::cartographer_ros::TrajectoryOptions trajectory_options;
        };
        ::std::vector<float> split(const ::std::string &s, char delim) {
            ::std::vector<float> elems;
            ::std::stringstream ss(s);
            ::std::string number;
            while (::std::getline(ss, number, delim)) {
                elems.push_back(atof(number.c_str()));
            }
            return elems;
        }
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        bool SerializeState(const std::string& filename,cartographer::mapping::MapBuilder*map_builder_) {
            cartographer::io::ProtoStreamWriter writer(filename);
            map_builder_->SerializeState(&writer);
            return writer.Close();
        }
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        void Run(const std::string& pbstream_filename, const std::string& map_filestem,
                 const double resolution) {
            ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
            ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
            const auto& pose_graph = deserializer.pose_graph();
            LOG(INFO) << "Loading submap slices from serialized data.";
            std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
                    submap_slices;
            ::cartographer::mapping::proto::SerializedData proto;
            while (deserializer.ReadNextSerializedData(&proto)) {
                if (proto.has_submap()) {
                    const auto& submap = proto.submap();
                    const ::cartographer::mapping::SubmapId id{
                            submap.submap_id().trajectory_id(),
                            submap.submap_id().submap_index()};
                    const ::cartographer::transform::Rigid3d global_submap_pose =
                            ::cartographer::transform::ToRigid3(
                                    pose_graph.trajectory(id.trajectory_id)
                                            .submap(id.submap_index)
                                            .pose());
                    FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
                }
            }
            CHECK(reader.eof());
            LOG(INFO) << "Generating combined map image from submap slices.";
            auto result =
                    ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
            ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
            ::cartographer::io::Image image(std::move(result.surface));
            cartographer::cartographer_ros::WritePgm(image, resolution, &pgm_writer);
            const Eigen::Vector2d origin(
                    -result.origin.x() * resolution,
                    (result.origin.y() - image.height()) * resolution);
            ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
            cartographer::cartographer_ros::WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
        }
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        static void OnLocalSlamResult(
                const int trajectory_id, const ::cartographer::common::Time time,
                const ::cartographer::transform::Rigid3d local_pose,
                ::cartographer::sensor::RangeData range_data_in_local,
                const ::std::unique_ptr<const ::cartographer::mapping::
                TrajectoryBuilderInterface::InsertionResult>) {
            //std::cout << "OnLocalSlamResult" << std::endl;
            ::std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
                    ::std::make_shared<TrajectoryState::LocalSlamData>(
                            TrajectoryState::LocalSlamData{time, local_pose,
                                                           ::std::move(range_data_in_local)});
            cartographer::common::Mutex mutex_;
            cartographer::common::MutexLocker lock(&mutex_);
            ::std::unordered_map<int, ::std::shared_ptr<const TrajectoryState::LocalSlamData>>
                    trajectory_state_data_ GUARDED_BY(mutex_);
            trajectory_state_data_[trajectory_id] = ::std::move(local_slam_data);
        }
        // For sensor_msgs::LaserScan.
        bool HasEcho(float) { return true; }
        float GetFirstEcho(float range) { return range; }
        template<typename LaserMessageType>
        ::std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time>
        LaserScanToPointCloudWithIntensities(const LaserMessageType &msg) {
            CHECK_GE(msg.range_min, 0.f);
            CHECK_GE(msg.range_max, msg.range_min);
            if (msg.angle_increment > 0.f) {
                CHECK_GT(msg.angle_max, msg.angle_min);
            } else {
                CHECK_GT(msg.angle_min, msg.angle_max);
            }
            ::cartographer::sensor::PointCloudWithIntensities point_cloud;
            float angle = msg.angle_min;
            for (size_t i = 0; i < msg.ranges.size(); ++i) {
                const auto &echoes = msg.ranges[i];
                if (HasEcho(echoes)) {
                    const float first_echo = GetFirstEcho(echoes);
                    if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
                        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
                        Eigen::Vector4f point;
                        point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
                                i * msg.time_increment;
                        point_cloud.points.push_back(point);
                        if (msg.intensities.size() > 0) {
                            CHECK_EQ(msg.intensities.size(), msg.ranges.size());
                            const auto &echo_intensities = msg.intensities[i];
                            CHECK(HasEcho(echo_intensities));
                            point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
                        } else {
                            point_cloud.intensities.push_back(0.f);
                        }
                    }
                }
                angle += msg.angle_increment;
            }
            ::cartographer::common::Time timestamp = ros::FromRos(msg.header.stamp);
            if (!point_cloud.points.empty()) {
                const double duration = point_cloud.points.back()[3];
                timestamp += ::cartographer::common::FromSeconds(duration);
                for (Eigen::Vector4f &point : point_cloud.points) {
                    point[3] -= duration;
                }
            }
            return ::std::make_tuple(point_cloud, timestamp);
        }
    }
};
int main(int argc, char *argv[]) {
    google::InitGoogleLogging(argv[0]);
    google::ParseCommandLineFlags(&argc, &argv, true);
    fflush(stdout);
    cartographer::cartographer_ros::NodeOptions node_options;
    cartographer::cartographer_ros::TrajectoryOptions trajectory_options;
//            std::string FLAGS_configuration_directory = "/home/cai/ros_test/catkin_ws4/catkin_ws/install_isolated/share/cartographer_ros/configuration_files";
    std::string FLAGS_configuration_directory = "/home/cai/CLionProjects/test/cartographer/configuration_files";
    std::string FLAGS_configuration_basename = "a2.lua";
    ::std::tie(node_options, trajectory_options) =
            cartographer::cartographer_ros::LoadOptions(FLAGS_configuration_directory,
                                                        FLAGS_configuration_basename);
    auto map_builder = ::cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
            node_options.map_builder_options);
    using SensorId = ::cartographer::mapping::TrajectoryBuilderInterface::SensorId;
    SensorId sensorid;
    sensorid.id = "range_sensor";
    sensorid.type = ::cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType::RANGE;
    ::std::set<SensorId> expected_sensor_ids;
    expected_sensor_ids.insert(sensorid);
    using LocalSlamResultCallback =
    ::cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback;
    //std::cout << "node_main_ map_builder->AddTrajectoryBuilde" << std::endl;
    const int trajectory_id = map_builder->AddTrajectoryBuilder(
            expected_sensor_ids, trajectory_options.trajectory_builder_options,
            ::std::bind(&cartographer::src::OnLocalSlamResult,
                        ::std::placeholders::_1, ::std::placeholders::_2,
                        ::std::placeholders::_3, ::std::placeholders::_4,
                        ::std::placeholders::_5));
//                    nullptr);
    auto trajectory_builder_ = map_builder->GetTrajectoryBuilder(trajectory_id);
    string sensor_id = "range_sensor";
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //
    ifstream myfile("/home/cai/bag/test928bag/xxx2.txt");
    ofstream outfile("/home/cai/bag/test928bag/fftestout2.txt", ios::app);
    string temp;
    if (!myfile.is_open()) {
        cout << "未成功打开文件" << endl;
    }
    vector<cartographer::cartographer_ros::LaserScan> laser_scans;
    int size = 0;
    cartographer::cartographer_ros::LaserScan scan;
    while (getline(myfile, temp)) {
        unsigned long i = temp.find("\n");
        int temp_size = temp.size();
        unsigned long iend = temp.find("---");
        if (iend == 0) {
            laser_scans.push_back(scan);
        }
        unsigned long iheader = temp.find("header: ");
        if (iheader == 0) {
        }
        unsigned long iseq = temp.find("  seq: ");
        if (iseq == 0) {
            string now_string = "  seq: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            int c_int = atoi(c_string.c_str());
            scan.header.seq = (uint32_t) c_int;
        }
        unsigned long istamp = temp.find("  stamp: ");
        if (istamp == 0) {
        }
        unsigned long isecs = temp.find("    secs: ");
        if (isecs == 0) {
            string now_string = "    secs: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            int c_int = atoi(c_string.c_str());
            scan.header.stamp.sec = (uint32_t) c_int;
        }
        //
        unsigned long insecs = temp.find("    nsecs: ");
        if (insecs == 0) {
            string now_string = "    nsecs: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            int c_int = atoi(c_string.c_str());
            scan.header.stamp.nsec = (uint32_t) c_int;
        }
        unsigned long iframe_id = temp.find("  frame_id: ");
        if (iframe_id == 0) {
            string now_string = "  frame_id: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            scan.header.frame_id = c_string;
        }
        unsigned long iangle_min = temp.find("angle_min: ");
        if (iangle_min == 0) {
            string now_string = "angle_min: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.angle_min = c_float;
        }
        unsigned long iangle_max = temp.find("angle_max: ");
        if (iangle_max == 0) {
            string now_string = "angle_max: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.angle_max = c_float;
        }
        unsigned long iangle_increment = temp.find("angle_increment: ");
        if (iangle_increment == 0) {
            string now_string = "angle_increment: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.angle_increment = c_float;
        }
        unsigned long itime_increment = temp.find("time_increment: ");
        if (itime_increment == 0) {
            string now_string = "time_increment: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.time_increment = c_float;
        }
        unsigned long iscan_time = temp.find("scan_time: ");
        if (iscan_time == 0) {
            string now_string = "scan_time: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.scan_time = c_float;
        }
        unsigned long irange_min = temp.find("range_min: ");
        if (irange_min == 0) {
            string now_string = "range_min: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.range_min = c_float;
        }
        unsigned long irange_max = temp.find("range_max: ");
        if (irange_max == 0) {
            string now_string = "range_max: ";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            float c_float = atof(c_string.c_str());
            scan.range_max = c_float;
        }
        unsigned long iranges = temp.find("ranges: [");
        if (iranges == 0) {
            string now_string = "ranges: [";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            const vector<float> &vector = cartographer::src::split(c_string, ',');
            scan.ranges = vector;
        }
        unsigned long iintensities = temp.find("intensities: [");
        if (iintensities == 0) {
            string now_string = "intensities: [";
            int now_int = now_string.size();
            int now_size = temp.size();
            string c_string = temp.substr(now_int, now_size);
            const vector<float> &vector = cartographer::src::split(c_string, ',');
//            std::cout<<vector.size()<<std::endl;
            scan.intensities = vector;
        }
    }
    std::cout<<laser_scans.size() <<std::endl;
    for (int i = 0; i < laser_scans.size(); ++i) {
        cartographer::cartographer_ros::LaserScan scan_t = laser_scans[i];
        //std::cout << scan_t.header.stamp.sec << std::endl;
       // std::cout << scan_t.header.stamp.nsec << std::endl;
       // std::cout << scan_t.header.frame_id << std::endl;
       // std::cout << scan_t.angle_min << std::endl;
       // std::cout << scan_t.angle_max << std::endl;
      //  std::cout << scan_t.angle_increment << std::endl;
       // std::cout << scan_t.time_increment << std::endl;
       // std::cout << scan_t.scan_time << std::endl;
       // std::cout << scan_t.range_min << std::endl;
       // std::cout << scan_t.range_max << std::endl;
    }
    myfile.close();
    outfile.close();
    //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    for (int j = 0; j < laser_scans.size(); ++j) {
        cartographer::cartographer_ros::LaserScan scan_msg = laser_scans[j];
        const tuple<cartographer::sensor::PointCloudWithIntensities, cartographer::common::Time> &tuple = cartographer::src::LaserScanToPointCloudWithIntensities(
                scan_msg);
        cartographer::sensor::PointCloudWithIntensities points;
        cartographer::common::Time commontime;
        std::tie(points, commontime) = tuple;
        using rep = int64_t ;
        using period = std::ratio<1, 10000000>;
        const std::chrono::duration<rep, period> &durationtime = commontime.time_since_epoch();
        std::cout << durationtime.count() << std::endl;
//            void SensorBridge::HandleLaserScan(
//                    const std::string& sensor_id, const carto::common::Time time,
//                    const std::string& frame_id,
//                    const carto::sensor::PointCloudWithIntensities& points) {
        std::map<std::string, cartographer::common::Time> sensor_to_previous_subdivision_time_;
        int num_subdivisions_per_laser_scan_ = 1;
        CHECK_LE(points.points.back()[3], 0);
        for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
            const size_t start_index =
                    points.points.size() * i / num_subdivisions_per_laser_scan_;
            const size_t end_index =
                    points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
            cartographer::sensor::TimedPointCloud subdivision(
                    points.points.begin() + start_index, points.points.begin() + end_index);
            if (start_index == end_index) {
                continue;
            }
            const double time_to_subdivision_end = subdivision.back()[3];
            // `subdivision_time` is the end of the measurement so sensor::Collator will
            // send all other sensor data first.
            const cartographer::common::Time subdivision_time =
                    commontime + cartographer::common::FromSeconds(time_to_subdivision_end);
            auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
            if (it != sensor_to_previous_subdivision_time_.end() &&
                it->second >= subdivision_time) {
                LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                             << sensor_id << " because previous subdivision time "
                             << it->second << " is not before current subdivision time "
                             << subdivision_time;
                continue;
            }
            sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
            for (Eigen::Vector4f &point : subdivision) {
                point[3] -= time_to_subdivision_end;
            }
            // CHECK_EQ(subdivision.back()[3], 0);
            //
            using Vector = Eigen::Matrix<double, 3, 1>;
            double a = 0;
            Vector Vtranslation(a, a, a);
            Eigen::Quaterniond rotation(a, a, a, a);
            //::std::unique_ptr<::cartographer::transform::Rigid3d> sensor_to_tracking;
            ::std::unique_ptr<::cartographer::transform::Rigid3d> sensor_to_tracking(
                    new ::cartographer::transform::Rigid3d(Vtranslation, rotation));
//                TfBridge(const std::string& tracking_frame,
//                double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer)
//                //我少了这一句
//                const auto sensor_to_tracking =
//                        tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(s_frame_id));
            if (sensor_to_tracking == nullptr) {
                std::cout << "sensor_to_tracking == nullptr" << std::endl;
            }
            if (sensor_to_tracking != nullptr) {
                trajectory_builder_->AddSensorData(
                        sensor_id, cartographer::sensor::TimedPointCloudData{
                                subdivision_time, sensor_to_tracking->translation().cast<float>(),
                                cartographer::sensor::TransformTimedPointCloud(subdivision,
                                                                               sensor_to_tracking->cast<float>())
                        });
            }
        }
    }
    cartographer::src::SerializeState("/home/cai/bag/test928bag/pbstream_test.pbstream",map_builder.get());
    cartographer::src::Run("/home/cai/bag/test928bag/pbstream_test.pbstream",
                           "/home/cai/bag/test928bag/pbstream_test",
                           0.1);
    for (const auto &submap :map_builder->pose_graph()->GetAllSubmapData()) {
//        cartographer::mapping::proto::SubmapQuery::Response response_proto;
//        cartographer::mapping::SubmapId submap_ied{trajectory_id, 0};
//        const std::string error =map_builder->SubmapToProto(submap_ied, &response_proto);
//        response_proto.textures();
        //
        const cartographer::mapping::Submap *pSubmap = submap.data.submap.get();
        std::cout << "pSubmap->num_range_data()" << pSubmap->num_range_data() << std::endl;
        cartographer::mapping::proto::Submap proto;
        cartographer::mapping::Submap2D *sub2d = (cartographer::mapping::Submap2D *) pSubmap;
        const cartographer::mapping::Grid2D *pD = sub2d->grid();
        const cartographer::mapping::proto::Grid2D &d = sub2d->grid()->ToProto();
        int i = d.cells_size();
        const google::protobuf::RepeatedField<google::int32> &field = d.cells();
//        for (int j = 0; j < field.size(); ++j) {
//            int m  = field.Get(j);
//            if (m!=0) {
//                std::cout<<field.Get(j) <<std::endl;
//            }
//        }
        int dayu0 = 0;
        for (int j = 0; j < field.size(); ++j) {
            int m = field.Get(j);
            if (m != 0) {
                int x = j / 200;
                int y = j % 200;
                dayu0++;
                if (dayu0%2 !=0) {
                    std::cout << "(" << x << "," << y << ")," ;
                }
            }
        }
        std::cout << "pD->getcorrespondence_cost_cells()=" << i << std::endl;
        std::cout << pD->limits().cell_limits().num_x_cells << std::endl;
        std::cout << pD->limits().cell_limits().num_y_cells << std::endl;
        pSubmap->ToProto(&proto, true);
        // proto.PrintDebugString();
    }
}
//                    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);

https://www.jianshu.com/p/92e917147256

Python量化投资网携手4326手游为资深游戏玩家推荐:《《消零世界》:【BW回顾】夏米尔和黛西在广州的奇妙经历!

「点点赞赏,手留余香」

    还没有人赞赏,快来当第一个赞赏的人吧!
0 条回复 A 作者 M 管理员
    所有的伟大,都源于一个勇敢的开始!
欢迎您,新朋友,感谢参与互动!欢迎您 {{author}},您在本站有{{commentsCount}}条评论