Catkinize

[devbharat]

I need to catkinize lsd-slam to use gtsam instead of g2o for map optimization. (Previously I had tried to include gtsam as a shared library in las-slam-core, but there is some issue with the version of eigen used by gtsam and lsd-slam.).

Have you tried to build lsd-slam with catkin ? I have gtsam as a catkin package. How convoluted would it be to build it via catkin ?

Thanks

[icoderaven]

Re: building using Catkin on Indigo, can you try out the indigo branch on https://github.com/icoderaven/lsd_slam that I just pushed?

[devbharat]

You mean try to catkinize it with the python script ?

[icoderaven]

No, I meant for just building LSD SLAM as-is using catkin. Once you've checked out that branch it should just build fine with a regular catkin_make.

Regarding your other question, from cursory inspection it doesn't seem trivial to use gtsam instead of g2o; you would have to implement setting the sim3 poses and constraints the way it is done with g2o at the moment (see the GlobalMapping source files). I may be wrong, though, since I don't know much about gtsam's implementation.

[devbharat]

Oh cool! Let me give building it a shot! It'll be amazing if it builds. Yep! You are right about the sim3 implementations in gtsam, i spent some time and implemented that gtsam, so I have now factors with Sim3 value type constraints working in gtsam.

The reason we decided to shift to gtsam was we'd like to build really long aerial maps which need to be 'consistent' upto only a few previous keyframes, so we'd like to marginalize over the previous nodes that are no longer optimized, sort of like a smooting window following behind the current keyframe. I also want to add gps position constraints (the problem without them is the scale optimization breaks every so often)

[icoderaven]

That's nice. I'd be interested to see your implementation if you do put it up on github, as I was planning to do something quite similar in the near future.

[devbharat]

Cool! It builds fine! But I am running into the same Eigen issue (with commainitializer) that I did when I tried to link gtsam as a shared lib in original rosbuild las-slam. I need to modify lsd_slam to use this version eigen https://github.com/ethz-asl/eigen_catkin branch fix/gtsam-eigen. Let me try that.

Sure! You can check it out at https://github.com/devbharat/gtsam. The Sim3 types in gtsam are called Moses3 for some funny reason. You can see a basic example in examples/LocationExample.cpp.

[icoderaven]

Well, I'm not sure if it helps, but you'd have to move away from the cmake_modules supplied Eigen, set the Eigen directory manually in your CMakeLists.txt, and then call find_package() on Eigen (and hopefully your Eigen fork should provide a cmake file when it installs)

[devbharat]

Oh yes! I made a little hack to modify the Eigen include path to the one i wanted and it works.

[icoderaven]

Sweet. :+1:

[devbharat]

Hey! I am not sure how familiar you are with lsd_slam(hopefully very!) could you guess how I could calculate the variance for Sim3 transitions for "right-composition" rather than "left-composition" as it's in their default code ? The paper http://vision.in.tum.de/_media/spezial/bib/engel2013iccv.pdf page '6' mentions it that it needs to be modified, but I got no clue how.

Thanks!

[icoderaven]

Hm, I think you meant their ECCV '14 LSD SLAM paper. Well, as far as the left/right thing goes, to me it just seems like a difference of implementation of how to add a delta transformation in SE3 space (essentially what convention you use for that compound operation). This paper approaches it like a pre-multiplication of a delta transformation, while g2o treats it like a post increment in se3 algebra (see equation 17 of http://europa.informatik.uni-freiburg.de/files/kuemmerle11icra.pdf) I don't think that g2o explicitly needs a right compositional input; for instance in their implementation, they use this convention to get a covariance matrix in the tracking method (after the Gauss-Newton) and push it to the graph. So unless you're implementing your tracking method yourself, you should be fine.

When it comes to adding information to the pose graph, yes, you need to be consistent, which is why they have the g2oTypeSim3Sophus custom vertices that define the compound operation (oplusImpl) and the corresponding Jacobian in the edge (linearizeOplus). So, say you had a hypothetical Information matrix obtained using the right compositional convention, all you would do is make sure that the above mentioned methods apply the increment afterwards (e.g. a quick dirty search leads me to http://rock-robotics.org/master/api/slam/g2o/classg2o_1_1VertexSE3.html#a82687fc06d36dce1e8472d5afee5aa0e)

Hope that helps! Also, since we seem to be doing this, let me know if you have any solution to incorporating the pose graph keyframe information into the mapping component in issue #48

[devbharat]

Hey! Thanks for the reply.

I agree that right/left composition to make increments in SE3/Sim3 space should both work, and my concern is regarding the 'consistency' of using the obtained covariance matrix in the pose-graph. lsd-slam 'as-is' does incremental composition on the left in all it's optimizations - SE3, Sim3 and then in pose-graph in g2o. And, as you said, they have modified the g2o's default right-composition convention in g2oTypeSim3Sophus.

Now, although I essentially use the same SE3 and Sim3Tracker of lsd-slam, the final map optimization is done not by g2o but by GTSAM. In addition to the Sim3 Factors, there are also GPS Factors to the keyframe nodes to bring in realworld scale and orientation.

It seems to work fine, catching the realworld scale and orientation, although I suppose the results could be better. I suspect that the way I have included the covariance matrix from the Sim3Tracker into the gaussian noise covariance of Sim3 Factors of GTSAM is incorrect.

GTSAM does right composition by default, and I am reluctant to modify it for left composition. SE3 tracker and Sim3Tracker do ls-optimization with left composition, and their obtained covariance matrices are suited for them. What, sort of a 'hack' I tried was to just switch the left-composition of Sim3Tracker to right-composition:

// apply increment. pretty sure this way round is correct, but hard to test.
Sim3 new_referenceToFrame =Sim3::exp(inc.cast<sophusType>()) * referenceToFrame;
//Sim3 new_referenceToFrame = referenceToFrame * Sim3::exp((inc));

(line 236: https://github.com/tum-vision/lsd_slam/blob/master/lsd_slam_core/src/Tracking/Sim3Tracker.cpp)

to the hacked version

// apply increment. pretty sure this way round is correct, but hard to test.
Sim3 new_referenceToFrame = referenceToFrame*Sim3::exp(inc.cast<sophusType>());
//Sim3 new_referenceToFrame = referenceToFrame * Sim3::exp((inc));

and used the then obtained covariance matrix in GTSAM. I am not sure if that little change completely accounts for the left-right composition or if I have to make these changes in SE3 tracker as well, etc.

What do you think ?

[icoderaven]

I'm not sure that that will solve it for you. For instance, you will at least have to make the same hack in SE3Tracker. Also, for instance, looking at how the use getCamToWorld(), I'm not sure if mixing the convention for rotation in the code will be a good idea.

Why is replicating what they did with g2oTypeSim3Sophus more difficult? I would definitely try that route first.

Also, define: the results could be better. Is it that the point clouds you are obtaining are not aligned together, or not the right scale?

[devbharat]

It does catch the right scale and orientation, but that is probably because gtsam trust GPS factors too much right now(the relative weighting of the gps and Sim3 factor noise covariance is sort of a free parameter, and a curved trajectory with known gps can make both scale and orientation observable).

That is probably why trying to use the covariance matrix obtained by both left and right composition in sim3tracker(line 236: https://github.com/tum-vision/lsd_slam/blob/master/lsd_slam_core/src/Tracking/Sim3Tracker.cpp) seems to give similar results, when I would expect at least one of them to totally screwup the gtsam's optimization.

The place where improvements are desired in the point cloud is the following. My dataset is one captured from a fixed wing UAV flying over 100m in a relatively 'texture-less' field, with some sections of the video having lots of 'usable pixels' with gradients, while others where they are quite few (like 20% of total pixels in the image). Also, the camera is looking ahead into it's direction of motion, so the epipole almost always lies inside the image, or near it's periphery, and the depth estimation around the epipole is quite poor. This leads to SE3 tracking 'breaks' at some points in the video (where the depth hypothesis for the particular keyframe isn't correct) and this incorrect SE3 edge leads to incorrect Sim3 edge which adds an incorrect edge to the graph. Without gps or any graph optimization, this was leading to sections of maps building to one scale - then tracking break at some point - then decent mapping at some other scale(usually much smaller than 1) - till another instance of tracking break etc.

We decide to add GPS to regain the correct scale after the tracking break, and have m-estimators on the Sim3edges in the graph so that incorrect outlier edges would be down-weighted in optimization. Also, once the SE3 tracking break occurs, I discard that keyframe's depth hypothesis and do a random depth init on it just like the actual initialization of the software. This corrected the issue with the scale, but now once the SE3 tracking break occurs, the orientation of the keyframes following the tracking break is screwed up.

I am not really sure why this is so, but having known incorrect covariance matrices in the sim3 factors definitely wouldn't help. I wasn't really leaning in the direction of modifying gtsam because I had spent quite sometime to test the sim3 factors etc and make sure they really work, and changing anything there would mean i'll have to do them all again. But it definitely can be done. It is probably even the easier way as you suggested.

[JakobEngel]

You can change the multiplication order of the Hessian matrix using the adjoint: lastSim3HessianRightEps = (referenceToFrame.inverse().Adj().transpose() lastSim3HessianLeftEps \ referenceToFrame.inverse().Adj()) ; (around line 363 in Sim3Tracker.cpp).

I'm only about 90% sure I got the .transpose() and the .inverse() right, that always confuses me.... You can derive it using the chain rule / propagation of uncertainty, and the definition of the Adjoint. Basically Adj_X for X in se(3) / sim(3) is a function (which, for lie-algebras, happens to be linear), that "moves" a left-multiplied epsilon on X to the right side of X (see http://en.wikipedia.org/wiki/Adjoint_representation).

Regarding the tracking loss: LSD-SLAM is quite stupid in that it simply uses a fixed threshold on the absolute gradient, to determine whether a pixel is considered at all. A better strategy would be to adaptively lower/raise that threshold, if too few/too many points are used. It should just remain high enough to not use pixels where the gradient mostly stems from noise.

Cheers, Jakob

[devbharat]

Ah! I got what you mean. I leave everything as is, sim3 tracker would still compose on the left. I can just calculate the covariance matrix for the right composition with the formula above. great!

Lastly, the order of Sim3 linear algebra elements in the Vec7 is translationx,y,z,rotation x,y,z, scale. Isn't it ?

[devbharat]

Hey @JakobEngel ! I was just looking at the formula you mentioned to transform the variance to the correct tangent space and comparing it against the one you've used for sim3constraint consistency check(your eccv paper eq. 20).

That implemented in Slamsystem.cpp(line 1095) looks like:

Matrix7x7 datimesb_db = AtoB.Adj();
Matrix7x7 diffHesse = (AtoBInfo.inverse() + datimesb_db * BtoAInfo.inverse() * datimesb_db.transpose()).inverse();

The relevant transformation being

(AtoB.Adj() * BtoAInfo.inverse() * AtoB.Adj().transpose())

So,

• You are transforming variance instead of information, and
• You've got .transpose() on the right, and
• For transforming the variance of BtoA, you are using the Adj() of AtoB

So, applying this for the Sim3tracker hessian, shouldn't the formula look like

lastSim3HessianRight = (referenceToFrame.inverse().Adj() * lastSim3HessianLeft.inverse() * referenceToFrame.inverse().Adj().transpose()).inverse();

I am still not sure about using referenceToFrame.inverse() or just referenceToFrame although, I am leaning towards the earlier. Can you @JakobEngel confirm this ?

@icoderaven Feel free to pitch in.

Thanks!

[nfnpmc]

No, the catkinized version still fails with a fatal error during compilation,

include "lsd_slam_core/LSDParamsConfig.h"

This header file is still not generated by something. I know not what or where. What have I missed?

[nfnpmc]

Ahha! I have determined that 'LSDParamsConfig.h' is supposed to be built by cfg/LSDParams.cfg when called from lsd_slam_core/CMakeLists.txt. From the tutorial on dynamic_reconfigure there is supposed to be a dependancy,

# make sure configure headers are built before any node using them
add_dependencies(example_node ${PROJECT_NAME}_gencfg)

but there isn't. The LSDDebugParams.cfg is also not built. (in the same CMakeLists) I am sure that you geniuses know what is going on, but I don't. How do I get them built? Do I add a dependency? on what node? Shame there isn't a Bug/Comment wiki on this.

[donald2016]

Try to follow this link http://visbot.blogspot.hk/2014/11/tutorial-building-of-lsd-slam-on-ros.html after building this, I got the error about missing LSDDebugParamsConfig.h LSDParamsConfig.h These files should generated by rosmake; however, I have met another problem of rosmake, so I downloaded it from https://github.com/ygling2008/dense_new/tree/master/src/lsd_slam_core

[ronir121]

I was wondering if the code of lsd-slam using gtsam and GPS measurements is available online? I went to https://github.com/devbharat/lsd_slam but didn't find any code with GPS or gtsam.