Getting the final poses

[Dcasadoherraez]

Thank you so much for providing the source code of your method! When you got the trajectory results for the paper, how did you get the trajectories from the SLAM pipeline? I've seen this issue in hdl_graph_slam (https://github.com/koide3/hdl_graph_slam/issues/15) However you already solved it for the paper, so it would be convenient to know how you did it :)

[Dcasadoherraez]

I created an additional service that saves the optimized poses. Only the optimized keyframes are saved.

bool save_poses_service(radar_graph_slam::SavePosesRequest& req, radar_graph_slam::SavePosesResponse& res) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    std::string directory = req.destination;

    if (directory.empty()) {
        std::array<char, 64> buffer;
        buffer.fill(0);
        time_t rawtime;
        time(&rawtime);
        const auto timeinfo = localtime(&rawtime);
        strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y_%H-%M-%S", timeinfo);
        directory = std::string(buffer.data());
    }

    if (!boost::filesystem::is_directory(directory)) {
        boost::filesystem::create_directory(directory);
    }

    std::cout << "Saving poses to: " << directory << std::endl;

    // Save flattened poses to a txt file
    std::ofstream poses_file(directory + "/poses.txt");
    if (!poses_file.is_open()) {
        std::cerr << "Failed to open file for saving poses!" << std::endl;
        res.success = false;
        return false;
    }

    poses_file << std::fixed << std::setprecision(6);

    for (const auto& keyframe : keyframes) {
        // Get the timestamp
        double timestamp = keyframe->stamp.toSec();

        // Get the transformation matrix
        Eigen::Matrix4d T = keyframe->node->estimate().matrix();

        // Flatten the matrix and save it along with the timestamp
        poses_file << timestamp;
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                poses_file << " " << T(i, j);
            }
        }
        poses_file << std::endl;
    }

    poses_file.close();

    std::cout << "All poses saved to: " << directory + "/poses.txt" << std::endl;

    res.success = true;
    return true;
}

[lushouyi]

Hello, based on your code, I have saved the keyframe pose after PGO optimization, but I found that the saved pose is the same as the pose from Scan2Scan. I don't know what went wrong. Could you help clarify my confusion?