Understanding how to get the cartographer maps

[lolhol]

Hi!

Basically I have this port of cartographer and I would like to understand how to get the maps more efficiently and prep them for like pathfinding and other stuff. Right now all I am doing is getting all of the submaps and making one big map. I am assuming that that is VERY inefficient in the way that I am using it right now (requesting it every like 0.5 sec).

My code right now that I run to get the map: `void CartoModule::paint_map(std::vector *output) { if (isAdded) { current_map->to_char_array(output); return; }

using io::PaintSubmapSlicesResult;
using io::SubmapSlice;
using mapping::SubmapId;

MapInfo info;
pthread_mutex_lock(&mutex);
double resolution = 0.05;

// 获取所有子图的位姿
std::map<SubmapId, SubmapSlice> submap_slices;
auto submap_poses = map_builder->pose_graph()->GetAllSubmapPoses();
for (const auto &submap_id_pose : submap_poses) {
    SubmapId submap_id = submap_id_pose.id;
    transform::Rigid3d pose = submap_id_pose.data.pose;
    int version = submap_id_pose.data.version;

    // 查询子图内容
    mapping::proto::SubmapQuery::Response response_proto;
    const std::string error = map_builder->SubmapToProto(submap_id, &response_proto);
    if (!error.empty()) {
        // LOG(ERROR) << error;
        pthread_mutex_unlock(&mutex);
        return;
    }
    int query_version = response_proto.submap_version();
    if (response_proto.textures_size() == 0) {
        // LOG(INFO) << "Responds of submap query is empty for submap '" << submap_id.submap_index << "'";
        continue;
    }
    // 提取第一个Texture
    auto first_texture = response_proto.textures().begin();
    std::string cells = first_texture->cells();
    int width = first_texture->width();
    int height = first_texture->height();
    resolution = first_texture->resolution();
    // LOG(INFO) << "############resolution=" << resolution<< "##############";
    transform::Rigid3d slice_pose = transform::ToRigid3(first_texture->slice_pose());
    auto pixels = io::UnpackTextureData(cells, width, height);

    // 填写SubmapSlice
    SubmapSlice &submap_slice = submap_slices[submap_id];
    submap_slice.pose = pose;
    submap_slice.metadata_version = version;
    submap_slice.version = query_version;
    submap_slice.width = width;
    submap_slice.height = height;
    submap_slice.slice_pose = slice_pose;
    submap_slice.resolution = resolution;
    submap_slice.cairo_data.clear();
    submap_slice.surface = ::io::DrawTexture(
        pixels.intensity, pixels.alpha, width, height,
        &submap_slice.cairo_data);
}  // for (const auto& submap_id_pose : submap_poses)

pthread_mutex_unlock(&mutex);
// LOG(INFO) << "Get and draw " << submap_slices.size() << " submaps";

// 使用Submap绘制地图
auto painted_slices = PaintSubmapSlices(submap_slices, resolution);
int width = cairo_image_surface_get_width(painted_slices.surface.get());
int height = cairo_image_surface_get_height(painted_slices.surface.get());

info.width = width;
info.height = height;
info.origen_x = -painted_slices.origin.x() * resolution;
info.origen_y = (-height + painted_slices.origin.y()) * resolution;
info.resolution = resolution;

current_map = std::make_unique<SimpleGridMap>(info);
uint32_t *pixel_data = reinterpret_cast<uint32_t *>(cairo_image_surface_get_data(painted_slices.surface.get()));
current_map->datas.reserve(width * height);
for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
        const uint32_t packed = pixel_data[y * width + x];
        const unsigned char color = packed >> 16;
        const unsigned char observed = packed >> 8;
        const int value = observed == 0 ? -1 : common::RoundToInt((1. - color / 255.) * 100.);
        CHECK_LE(-1, value);
        CHECK_GE(100, value);
        current_map->datas.push_back((char)value);
    }
}

// LOG(INFO) << "Paint map with width " << width << ", height " << height << ", resolution " << resolution;
//  isAdded = true;

current_map->to_char_array(output);

};`

I noticed that the "LocalSlamResult" has this thing called "const std::unique_ptr<const mapping::TrajectoryBuilderInterface::InsertionResult> insertion_result." I don't quite understand what that is but my hypothesis is that it is like the submap to which change was made. Am I correct? If so, how can I use it to not have to run the whole rendering of the map many times per second and only run it for a specific submap [that was changed].

My goal is to make a robot that auto-pathfinds around the room and stuff. Although I got the cartographer to work, the only way I can get the map is through that massive function that is (I am assuming) very slow and is not how you are supposed to do it. Is there a proper way to get the map in a 1D vector format with the ints (chars/bytes whatever) representing the color / obstructed not obstructed?

[lolhol]

Still don't know how to do this but I have moved to 3D so I am very desperately needing some help since the code that I found previously likely doesn't work with 3D.

I guess my new (ish kind of same thing) question is how I can get all the 3D points positions.

I found this https://github.com/cartographer-project/cartographer_ros/issues/1168 issue and one of the people says that you can call PoseGraphInterace::GetTrajectoryNodes() to render the 3d stuff. I'm not sure what this returns.

Could someone explain to me, please?

[graiola]

Hi @lolhol I am trying to get 3d maps as well. I started from #720 ad added a publisher for the 3d map. You can find the code at my fork https://github.com/graiola/cartographer_ros/tree/submap_cloud_query

If you set in your .lua pointcloud_map_publish_period_sec = 5., you should see the /map_3d topic being populated.

[lolhol]

Hi @lolhol I am trying to get 3d maps as well. I started from #720 ad added a publisher for the 3d map. You can find the code at my fork https://github.com/graiola/cartographer_ros/tree/submap_cloud_query

If you set in your .lua pointcloud_map_publish_period_sec = 5., you should see the /map_3d topic being populated.

Ok! This is a very big help! I coded a sample function thing based on your map_builder_bridge code. If it is not hard for you, could you please explain what a HybridGrid is and how it relates to min_probability? Because I am very new to cartographer and I have no clue on how it works.

Here is my code for the function.

`std::vector<std::vector> CreatePCLPointCloudFromHybridGrid( const cartographer::mapping::proto::HybridGrid &hybrid_grid, double min_probability) { std::vector<std::vector> point_cloud = std::vector<std::vector>();

double resolution = hybrid_grid.resolution();

for (int i = 0; i < hybrid_grid.values_size(); i++) { int value = hybrid_grid.values(i); if (value > 32767 * min_probability) { int x, y, z; x = hybrid_grid.x_indices(i); y = hybrid_grid.y_indices(i); z = hybrid_grid.z_indices(i);

  // Transform the cell indices to an actual voxel center point
  Eigen::Vector3f point = Eigen::Vector3f(x * resolution + resolution / 2,
                                          y * resolution + resolution / 2,
                                          z * resolution + resolution / 2);

  int prob_int = hybrid_grid.values(i);
  point_cloud.push_back({point.x(), point.y(), point.z(), static_cast<float>(prob_int / 32767.0)});
}

}

return point_cloud; }`

Also, is there possibly a way to get a specific part of a submap and only like get those points? because I plan to update the map pretty often and I can imagine that it will take significant time to iterate over each point in a map (especially if there is a big map). Is there a way to possibly get the map from the pose update callback you pass into the Trajectory builder?

[lolhol]

Also, I forgot to mention that I am not using ROS.

[lolhol]

Ok @graiola, so the code kinda works. Unfortunately, though it returns values like [-1.14080808E8, 0.0, 3218.45, 6.25] (x, y, z, i). I don't understand how that is possible since the resolution is set to 0.1 (I made it print the resolution and it outputted "resolution: 0.1"), and since I am feeding it data like -4.4900255 0.9554584 0.103424944 152.0 (x, y, z, i).

Any idea of what is going on? I don't really understand much here especially when it comes to actually rendering the map. Sorry if this is a stupid question. :/

If you are interested in my code, here is my github link. I will apologize in advance for possibly bad C++ code. I don't usually code in this lang and I still getting to know it. At any rate, the files for 3D stuff are in ./util/util3D

https://github.com/lolhol/GooglePotatoForJava

[lolhol]

Still no progress :/ any help?

Please...?

[lolhol]

Hi!

Some help would be great. Here is a photo of what I am getting as a map so far ->

(I know that I am scaling up the distance between points)

Are there any like 3D point viewers out there that I can use to verify my data?