Comments (8)
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).
from se-sync.
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
from se-sync.
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 ingraph_slam->graph
are already labeled starting at zero, you would need to relabel them to start at 1 in order to assignx_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 framex_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 bex_0 = (t_0, R_0) = (0, I)
, then the previous expression reduces tot_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 findt_j0 = R_j^{-1} (t_0 - t_j)
, which reduces tot_j0 = -R_j^{-1} t_j
under the assumption thatx_0 = (0, I)
; that is, this would give the robot's position at frame j, multiplied by the negative of its orientationR_j
. So the punchline is: I think you might want to try swapping the assignments toi
andj
in the second code block ;-).
Hope this helps -- please do let me know how it goes if you end up trying this!
from se-sync.
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.
from se-sync.
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
from se-sync.
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.
from se-sync.
@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;
}
}
from se-sync.
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.
from se-sync.
Related Issues (20)
- Synchronizing 2D rotations in 3D space HOT 1
- Sequence of recording vertices and edges in g2o file HOT 6
- liblapack.so.3: undefined references HOT 3
- add prior constraint HOT 7
- Question regarding SE-Sync vs. Shonan Rotation Averaging HOT 4
- Where can I find the optimized pose? HOT 2
- Where is the ground truth poses for the data folder? HOT 1
- G2o parsing code incorrectly names a variable as covariance instead of info matrix. HOT 1
- sometimes sesync may output incorrect results HOT 2
- Documentation on how to start HOT 5
- can se-sync work with factor graph generated with GSAM ? HOT 1
- Unable to enter visual mode
- The newest/accelerating version compiled in ubuntu 20.04... HOT 3
- How to optimize a least square problem, where there is only one node with multiple prior factors?
- Python binding installation and Inference
- SESync results matrix sometimes flips HOT 1
- can se sync solve BA or PnP?
- Cross compile for other platforms
- Which version of Pangolin should I use
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from se-sync.