return the entire map from the pose graph?

[antithing]

Hi, and thank you for making this code available. I am trying to use your pose graph implementation in my own non-ros application (based on BLAM).

I am running this to initialise:

pose_optimizer->setInitialPose(zeroPose);

then adding factors every frame with:

pose_optimizer->addNewFactor(&msg, PoseInMap, odometryPoseEst, &curr_vertex_key, new_keyframe);

Is this all i need to do to optimise the map?

How can i return the entire, optimised map?

Should I use something like:

for (size_t i = 0; i < curr_vertex_key_; ++i)
    {
        if (graph_scans_.count(i) == 1)
        {
            Pose6DOF keyframe_pose = graph_poses_.at(i);
            PointCloud::Ptr cloud_ptr = graph_scans_.at(i);
            PointCloud::Ptr cloud_out_ptr(new PointCloud());

            Eigen::Matrix4d tf_keyframe_in_map = keyframe_pose.toTMatrix();

            pcl::transformPointCloud(*cloud_ptr, *cloud_out_ptr, tf_keyframe_in_map);

                      //merge all graph scans into one cloud

or is there an easier way?

I see there is also a g2o optimisation implementation, is this functional as well?

Thank you very much for your time.

[YoshuaNava]

Hi antithing, Thank you for your interest.

This was a project I implemented for a Masters' course. It 1.5 months of work, and covers multiple aspects of Graph SLAM with a spinning Lidar (front end, back end, local loop closure + live visualization of results). The code is not the best, if I had known by then, I would have done many things in a very different way. About one month ago I pushed another version of the code that I had in my disk, and is way cleaner. You can find it in the fix/cleaner_interface branch. I don't think I'll have much more time to improve it, but if you have more questions, I'll be glad to answer them.

Answers to your questions:

1. If you want to optimize the map, you need to call the corresponding optimization method of gtsam/g2o, which is done every N keyframes.
2. If you want to access the latest map, you just have to query the mapcloud object that belongs to the OctreeMapper class
3. The g2o implementation is outdated. I moved away from g2o during my project because I found some issues with the optimization after a certain commit from June/July 2016.

[antithing]

Thanks! I will look at that branch. Can you please confirm that I am adding factors ciorrectly?

in the line:

pose_optimizer->addNewFactor(&cloud, icp_transform, icp_odom_pose, &curr_vertex_key, is_keyframe);

cloud is the current frame point cloud, icp_transform is the frame to frame transform, icp_odom_pose is the additive pose from the odometry, is that right?

[antithing]

Ah sorry, in the other branch this has been replaced with addNewKeyframe, that makes more sense. thanks again!

[antithing]

Hi again, i am looking at this other branch, and stripping out ROS. I have it building and compiling, but when i view:

`viewer2.showCloud(octree_mapper_.map_cloud_, "viewer1");`

The point cloud is flat, so every point's position in Z is 0.

Why might this be happening?

thanks!

[YoshuaNava]

HI Antithing, The z coordinates of the points should be full-3D. How are you feeding the algorithm? Are you using a 2D or 3D lidar?

[antithing]

Hi! i am using 3d Lidar, the kitti dataset. I have removed ROS, so I may have broken something in the process! My main function is:

void IcpSlam::mainLoop() {

    pcl::visualization::CloudViewer viewer2("viewer1");
    viewer2.runOnVisualizationThread(viewerPsycho);

 std::cout << "IcpSlam: Main loop started" << std::endl;

  bool run_pose_optimization = false, new_transform_icp = false, new_transform_rodom = false;
  unsigned long num_vertices = 1, iter = 0;

  Pose6DOF robot_odom_transform, robot_odom_pose, prev_robot_odom_pose, icp_transform, icp_odom_pose, prev_icp_odom_pose, prev_icp_pose;

  // We start at the origin
  std::cout << "IcpSlam:    Initial pose" << std::endl;
  prev_icp_odom_pose = Pose6DOF::getIdentity();
  prev_icp_pose = prev_icp_odom_pose;
  prev_robot_odom_pose = prev_icp_odom_pose;
 long long latest_stamp = 0.0;

 vector<string> files;
 std::string pathh = "X:/dev/sandbox_bsharp/data/data_odometry_velodyne/dataset/sequences/03/velodyne";
 getFiles(pathh, files);

 std::sort(files.begin(), files.end(), std::less<string>());// After sort will be ascending order

 icp_odometer_.setInitialPose(icp_odom_pose);

 for (int i = 0; i < files.size(); i++) {

     // load point cloud
     fstream input(files[i], ios::in | ios::binary);
     if (!input.good()) {
         cerr << "Could not read file: " << files[i] << endl;
         exit(EXIT_FAILURE);
     }
     input.seekg(0, ios::beg);

     pcl::PointCloud<PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
     points->clear();
     int j;
     for (j = 0; input.good() && !input.eof(); j++) {
         PointXYZ point;
         input.read((char *)&point.x, 3 * sizeof(float));
        // input.read((char *)&point.intensity, sizeof(float));
         points->push_back(point);
     }

     input.close();

    computeMapToOdomTransform();

    icp_odometer_.laserCloudCallback(points);

    if (icp_odometer_.isOdomReady()) {
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
      icp_odometer_.getEstimates(latest_stamp, cloud, icp_transform, icp_odom_pose, new_transform_icp);
      if (new_transform_icp) {
        if (num_keyframes_ > 0) {
          Pose6DOF refined_transform;
          std::cout << "IcpSlam: Transform refinement using nearest neighbor search" << std::endl;
          bool registration_success = octree_mapper_.refineTransformAndGrowMap(latest_stamp, cloud, prev_icp_odom_pose, refined_transform);
          if (registration_success) {
            std::cout <<"IcpSlam: Refinement successful" << std::endl;
            icp_transform = refined_transform;
          }
          icp_odom_pose = prev_icp_odom_pose + icp_transform;
        }

        if ((Pose6DOF::distanceEuclidean(icp_odom_pose, prev_icp_pose) > KFS_DIST_THRESH) || (num_keyframes_ == 0)) {
          std::cout << "IcpSlam:    Number of keyframes = " << num_keyframes_ + 1 << std::endl;
          addNewKeyframe(num_keyframes_, latest_stamp, icp_odom_pose, cloud);
          num_keyframes_++;
          num_vertices++;
          prev_icp_pose = icp_odom_pose;
          if (num_keyframes_ % keyframes_window_ == 0) {
            run_pose_optimization = true;
          }
        }
        new_transform_icp = false;
      }

      if (run_pose_optimization) {
        // octree_mapper_.resetMap();
        pose_graph_->optimize(true);
        run_pose_optimization = false;
      }

     // publishPoseGraphMarkers(ros::Time::now());

      prev_icp_odom_pose = icp_odom_pose;

    }

    viewer2.showCloud(octree_mapper_.map_cloud_, "viewer1");

    iter++;
  }
}

This gives me:

IcpSlam:    Number of keyframes = 1
vertex id = 0
    ID set
    SET FIXED
  NEW VERTEX READY
IcpSlam: Transform refinement using nearest neighbor search
IcpSlam: Octree map is empty!
IcpSlam: Transform refinement using nearest neighbor search
IcpSlam: Refinement successful
IcpSlam:    Number of keyframes = 2
vertex id = 1
    ID set
  NEW VERTEX READY
IcpSlam: Transform refinement using nearest neighbor search
IcpSlam: Refinement successful
IcpSlam: Transform refinement using nearest neighbor search
IcpSlam: Refinement successful
IcpSlam:    Number of keyframes = 3
vertex id = 2
    ID set
  NEW VERTEX READY
IcpSlam: Transform refinement using nearest neighbor search

What have I done wrong here? Thank you again!