Question regarding prior constraints

[UriyMihailov]

@david-m-rosen, thank you for your work, it has really helped me improve the overall result of the map creation algorithm. I have a question regarding prior constraints that have already been mentioned here: add prior constraints. I have a number of edges of type g2o::EdgeSE3PriorXYZ. They are used to store the GPS data and contain XYZ coordinates and a small 3x3 information matrix. Since I know that the SE-Sync algorithm can't work with such data, I instead complete them by copying their data into SESync::RelativePoseMeasurement variables and setting the remaining data (about the orientation) to 0. However, when I try to use such edges in the SE-Sync algorithm, it it completely destroys the entire map. Here's the visual example to better understand:

That one was when I was adding such edges. And this one is without them:

As you can see, adding those edges make the map worse. But I can't NOT add them as they are needed later in other applications. So I wanted to ask your advice if there was something I programmed incorrectly? Here's the necessary code:

SESync::measurements_t read_graph_edges() { SESync::measurements_t measurements; SESync::RelativePoseMeasurement measurement;

for(auto& edge : graph_slam->graph->edges())
{
  if(edge->vertices().size() == 2)
  {
    measurement.i = edge->vertices()[0]->id();
    measurement.j = edge->vertices()[1]->id();
    //std::cout << measurement.i << " " << measurement.j << std::endl;

    g2o::EdgeSE3* ed = dynamic_cast<g2o::EdgeSE3*>(edge);
    Eigen::Isometry3d iso = ed->measurement();
    Eigen::MatrixXd info = ed->information();
    measurement.t = iso.translation();
    measurement.R = iso.rotation();
    //std::cout << measurement.t << std::endl;
    //std::cout << measurement.R << std::endl;

    Eigen::Matrix3d TranInfo;
    TranInfo(0, 0) = info(0, 0); TranInfo(0, 1) = info(0, 1); TranInfo(0, 2) = info(0, 2);
    TranInfo(1, 0) = info(1, 0); TranInfo(1, 1) = info(1, 1); TranInfo(1, 2) = info(1, 2);
    TranInfo(2, 0) = info(2, 0); TranInfo(2, 1) = info(2, 1); TranInfo(2, 2) = info(2, 2);
    measurement.tau = 3 / TranInfo.inverse().trace();
    //std::cout << measurement.tau << " ";
    Eigen::Matrix3d RotInfo;
    RotInfo(0, 0) = info(3, 3); RotInfo(0, 1) = info(3, 4); RotInfo(0, 2) = info(3, 5);
    RotInfo(1, 0) = info(4, 3); RotInfo(1, 1) = info(4, 4); RotInfo(1, 2) = info(4, 5);
    RotInfo(2, 0) = info(5, 3); RotInfo(2, 1) = info(5, 4); RotInfo(2, 2) = info(5, 5);
    measurement.kappa = 3 / (2 * RotInfo.inverse().trace());
    //std::cout << measurement.kappa << std::endl;
    //std::cout << "\n";

    measurements.push_back(measurement);
  }
  else if(edge->vertices().size() == 1)
  {
    measurement.i = edge->vertices()[0]->id();
    measurement.j = 0;
    //std::cout << measurement.i << " " << measurement.j << std::endl;

    g2o::EdgeSE3PriorXYZ* ed = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
    Eigen::Vector3d vec = ed->measurement();
    Eigen::MatrixXd info = ed->information();
    measurement.t = vec;
    measurement.R = Eigen::Matrix3d::Identity();
    //std::cout << measurement.t << std::endl;
    //std::cout << measurement.R << std::endl;

    measurement.tau = 3 / info.inverse().trace();
    //std::cout << measurement.tau << " ";
    measurement.kappa = 0;
    //std::cout << measurement.kappa << std::endl;
    //std::cout << "\n";

    measurements.push_back(measurement);
  }
}

return measurements;

}

The rest of the code is a copy of SE-Sync's main.cpp file. The else if(edge->vertices().size() == 1) part is where I work with these edges. Thanks for your help.

[david-m-rosen]

Hi @UriyMihailov,

Thanks for your interest in the project! (And apologies for the very long delay in getting back to you -- I've been on travel for the last couple of weeks [first for ICRA, and then for some personal stuff], and so haven't been super responsive via email ...)

Overall it looks like your code is mostly doing the right thing -- in particular, I agree that your idea of using a "virtual" rotational measurement and then setting the corresponding rotational measurement precision (kappa) to 0 should work. A couple of things I would suggest you double-check:

• The approach described in my previous comment requires adding a virtual pose x_world that represents the absolute world coordinate frame. It's not obvious to me from the above code snippet whether your code is actually doing this. (For example, if the poses in graph_slam->graph are already labeled starting at zero, you would need to relabel them to start at 1 in order to assign x_world the label 0 (as you do when processing the GPS measurements in the second code block).

• I did spot what I think could be a subtle bug in the second code block. Remember that the relative measurements x_ij = (t_ij, R_ij) are measurements of the relative transform from frame i to frame j. An absolute measurement of the robot's position at pose x_j would then correspond to a measurement of the translation from the world frame x_0 to frame j. You can see this more explicitly by writing out the translational part of the measurement x_0j := x_0^-1 * x_j: t_0j = R_0^{-1} (t_j - t_0). Note that if we take the world frame to be x_0 = (t_0, R_0) = (0, I), then the previous expression reduces to t_0j = t_j; that is, this corresponds to a measurement of the robot's (absolute) position tj, which is exactly what we want. On the other hand, if we consider the translational part of x_j0, we find t_j0 = R_j^{-1} (t_0 - t_j), which reduces to t_j0 = -R_j^{-1} t_j under the assumption that x_0 = (0, I); that is, this would give the robot's position at frame j, multiplied by the negative of its orientation R_j. So the punchline is: I think you might want to try swapping the assignments to i and j in the second code block ;-).

Hope this helps -- please do let me know how it goes if you end up trying this!

[UriyMihailov]

Hey, @david-m-rosen, thank you for your answer. Sorry it took me so long to answer, I've had a lot of things to deal with, so I only recently had some spare time to try out your suggestion. It seems it actually solved the issue, since the map doesn't get so corrupted and destroyed anymore and is actually pretty smooth now. As for the x_world: there actually is a frame 0, it is connected with frame 1 and serves only one purpose of keeping the map in place, so it doesn't move around. This frame is called the anchor and is fixed in one place, although it does allow the map to rotate. Since this frame stays in one place, I decided I could use it as a world coordinate frame. I include it into the optimizer, then get the result matrix, find the inverse matrix and multiply every frame by it, including the anchor.

[UriyMihailov]

Here is the end result, if you're interested:
As you can see, the map is complete, not heavily corrupted, all the buildings are distinguishable (the graph with the map was loaded into the interactive slam project made by koide3).

[david-m-rosen]

Hi @UriyMihailov,

Thanks very much for following up -- the new map looks great! Glad we were able to get that sorted out :-).

And please do feel free to ping me again whenever the project you're working on is ready for prime-time! It's always really fun to see how people are using the library :-).

All the best, -Dave

[thachhoang1909]

Thanks @david-m-rosen for your great work on this.

Hi @UriyMihailov, your work on integrating GPS information is very interesting and the result looks promising. I wonder whether it is possible to share your source code.

Thank you and hope to hear from you, Thach

[michael-sz]

SESync::measurements_t read_graph_edges()

hi,

Thanks @david-m-rosen for your great work on this.

Hi @UriyMihailov, your work on integrating GPS information is very interesting and the result looks promising. I wonder whether it is possible to share your source code.

Thank you and hope to hear from you, Thach @david-m-rosen great job! @UriyMihailov hi, now i am learning how to use se-sync as back-end optimiztion, i think your source code may help me, i wonder whether it is possible to share your source code too. thanks.

[UriyMihailov]

@thachhoang1909 , there's actually nothing special about my code. The first function is the one I've already shown: it creates a bunch of measurements. The second one is the graph optimizer: I've already had one optimizer, and so I've added an IF statement about the chosen optimizer. So the code for SE-Sync looks like this:

else if(optimizator == "SESync")
{
  if(graph_slam->graph->edges().size() >= 10)
  {      
    std::cout << "SESync started" << std::endl;
    SESync::measurements_t measurements = read_graph_edges();
    std::cout << measurements.size() << std::endl;
    std::cout << "Measurements taken" << std::endl;

    SESync::SESyncOpts opts;
    opts.verbose = false;
    opts.initialization = SESync::Initialization::Chordal;
    opts.formulation = SESync::Formulation::Simplified;
    opts.num_threads = 4;
    std::cout << "Options created" << std::endl;

    SESync::SESyncResult results = SESync::SESync(measurements, opts);
    std::cout << "Results acquired" << std::endl;

    int vertices_size = graph_slam->graph->vertices().size();
    Eigen::Isometry3d shift = Eigen::Isometry3d::Identity();

    for(auto vertex : graph_slam->graph->vertices())
    {
      int id = vertex.second->id();

      g2o::VertexSE3* ver = dynamic_cast<g2o::VertexSE3*>(vertex.second);
      Eigen::Isometry3d iso = Eigen::Isometry3d::Identity();

      iso(0, 0) = results.xhat(0, vertices_size + (id * 3)); iso(0, 1) = results.xhat(0, vertices_size + (id * 3) + 1); iso(0, 2) = results.xhat(0, vertices_size + (id * 3) + 2); iso(0, 3) = results.xhat(0, id);
      iso(1, 0) = results.xhat(1, vertices_size + (id * 3)); iso(1, 1) = results.xhat(1, vertices_size + (id * 3) + 1); iso(1, 2) = results.xhat(1, vertices_size + (id * 3) + 2); iso(1, 3) = results.xhat(1, id);
      iso(2, 0) = results.xhat(2, vertices_size + (id * 3)); iso(2, 1) = results.xhat(2, vertices_size + (id * 3) + 1); iso(2, 2) = results.xhat(2, vertices_size + (id * 3) + 2); iso(2, 3) = results.xhat(2, id);

      ver->setEstimate(iso);
      if(id == 0)
      {
        shift = iso;
      }
    }

    shift = shift.inverse();
    for(auto vertex : graph_slam->graph->vertices())
    {
      g2o::VertexSE3* ver = dynamic_cast<g2o::VertexSE3*>(vertex.second);
      ver->setEstimate(shift * ver->estimate());
      if(ver->id() == 0)
      {
        Eigen::Isometry3d temp = ver->estimate();
        std::cout << temp(0, 0) << " " << temp(0, 1) << " " << temp(0, 2) << " " << temp(0, 3) << std::endl;
        std::cout << temp(1, 0) << " " << temp(1, 1) << " " << temp(1, 2) << " " << temp(1, 3) << std::endl;
        std::cout << temp(2, 0) << " " << temp(2, 1) << " " << temp(2, 2) << " " << temp(2, 3) << std::endl;
        std::cout << temp(3, 0) << " " << temp(3, 1) << " " << temp(3, 2) << " " << temp(3, 3) << std::endl;
      }
    }
    std::cout << "Vertices updated" << std::endl;
  }
}

[UriyMihailov]

Oh, by the way, @david-m-rosen , I wanted to say that, while your project works perfectly with ROS1, in it's current state it completely refuses to work with ROS2. To make it work I've had to completely remake its structure from scratch as well as make some changes inside the files. Please don't think that I'm trying to blame you or offend you, I'm just stating a fact so that those who will come later with questions about this have something to start from. I would like to put it to my GitHub folder but I'm not sure if I'm allowed to due to the company's policies.