[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion #2619

akashsharma02 · 2020-07-29T20:27:14Z

This work is part of GSoC and is for the 2nd and 3rd part of the evaluation phase of the program.

My mentor for the GSoC time period is: @savuor
My proposal is available here: https://summerofcode.withgoogle.com/dashboard/project/6190777371197440/details/

Checklist before review:

Track multiple submaps simultaneously
Posegraph data structure and Optimizer (Uses Ceres for PoseGraph optimization)
Add support for computing constraints between submaps (In progress/Partially done)
Camera Relocalization (Dropping for GSoC. Will take it up after evaluation)

Pull Request Readiness Checklist

I agree to contribute to the project under OpenCV (BSD) License.
To the best of my knowledge, the proposed patch is not based on code under GPL or another license that is incompatible with OpenCV
The PR is proposed to the proper branch
The feature is well documented and sample code can be built with the project CMake

Merge opencv master

- Implement Integrate function (untested)

- Clean up comments and few fixes

- Format code - Delete kinfu_back.cpp

- Address Review comments

- Fix formatting according to comments - Move volume abstract class to include/opencv2/rgbd/ - Write factory method for creating TSDFVolumes - Small bug fixes - Minor fixes according to comments

- Fix raycasting bug causing to loop forever - Suppress warnings by explicit conversion - Disable hashTsdf test until we figure out memory leak - style changes - Add missing license in a few files, correct precomp.hpp usage

HashTSDF volume

Implement overlap_ratio check to create new submaps

(Huber threshold based inliers pending)

submaps

- Simplify calculation of visibleBlocks

savuor · 2020-08-05T14:20:56Z

modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

+
+  That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
+*/
+class CV_EXPORTS_W LargeKinfu


Docs should be rewritten later

Have modified the docs a bit. Do let me know if there are any concerns or issues.

savuor · 2020-08-05T14:23:21Z

modules/rgbd/include/opencv2/rgbd/volume.hpp

-          voxelSizeInv(1.0f / voxelSize),
-          pose(_pose),
-          raycastStepFactor(_raycastStepFactor)
+        : voxelSize(_voxelSize), voxelSizeInv(1.0f / voxelSize), pose(_pose), raycastStepFactor(_raycastStepFactor)


The code in this file is under active refactoring in another PR, a rebase will be required later.

savuor · 2020-08-05T14:24:15Z

modules/rgbd/include/opencv2/rgbd/volume.hpp

+    int unitResolution = 0;
+
+    /** @brief Initial pose of the volume in meters */
+    cv::Affine3f pose;


Please use Matx44f instead, Affine3f has problems with Python bindings

As far as I know Affine3f coerces the matrix into SE3, i.e., the R matrix has determinant always +1, so changing to Matx44f is not advisable I think.

In fact nothing stops a user from spoiling Affine3f since:

it can be constructed from arbitrary 4x4 matrix (or 3x3 R + 1x3 t matrices)

it keeps the same Matx44f inside as a public member
So in both cases a user should be careful about the value.

Done. Changed most interfaces into Matx44f. Personally though I think we should consider adding a proper SE3 pose class

There are plans to add quaternions and possibly dual quaternions into OpenCV 5.0. If they are done properly, there will be consistent data structures to keep both SO(3) and SE(3) transforms.

savuor · 2020-08-05T14:26:06Z

modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

+
+    CV_WRAP virtual void reset() = 0;
+
+    virtual const Affine3f getPose() const = 0;


Please use Matx44f instead, Affine3f has problems with Python bindings

savuor · 2020-08-05T14:27:53Z

modules/rgbd/samples/large_kinfu_demo.cpp

+#include <opencv2/viz.hpp>
+#endif
+
+static vector<string> readDepth(std::string fileList);


I think common pieces from all demos like DepthSource etc. can be moved to a single file later.

Done this, and moved code for kinfu_demo and large_kinfu_demo into a single io_utils.hpp file. Have not modified dynafu_demo code. Let me know if that is required.

Great! Please move this header file from public headers to samples directory, I think the functions it contains make a little sense without these demos.

You may also modify dynafu_demo.

savuor · 2020-08-06T18:09:49Z

modules/rgbd/src/submap.hpp

+
+        PoseConstraint() : weight(0){};
+
+        void accumulatePose(const Affine3f& _pose, int _weight = 1)


Is this pose averaging method good enough for constraint estimation? I mean, rotation matrix average is almost never a rotation matrix again. If you need a good method for pose averaging, I can provide you with DQB src which I'm going to integrate in one of my next PRs.
Or is this used just to estimate mean/stddev for matrix coeffs? If so, why not collecting axis-angle vectors instead?

I think this pose averaging method is good enough. Here, the resulting matrix always represents the same relative pose and is coerced back into SE3 using Affine3f I think. Correct me If I'm wrong. Let's discuss this offline.

savuor · 2020-08-06T18:15:38Z

modules/rgbd/src/submap.hpp

+                inlierConstraint += fromSubmapData.constraints[i].matrix;
+        }
+    }
+    inlierConstraint /= float(max(localInliers, 1));


The same about average pose

savuor · 2020-08-06T18:32:38Z

modules/rgbd/src/volume.cpp

+            params.depthTruncThreshold = 0.f;  // depthTruncThreshold not required for TSDF
+            break;
+        case VolumeType::HASHTSDF:
+            params.unitResolution = 16;


Unused fields like resolution and unitResolution should be marked as such, like it's done with depthTruncThreshold

savuor · 2020-08-06T18:38:41Z

modules/rgbd/src/large_kinfu.cpp

+            //! Compose current pose and add to camera trajectory
+            currTrackingSubmap->composeCameraPose(affine);
+            std::cout << "Number of inliers: " << numInliers << "\n";
+        }


Should we continue if tracking failed?

Since we are tracking each submap independently, we should go forward. I am yet to add some code to remove the submap from active list, if the tracking fails consistently for a few frames, etc.

savuor · 2020-08-06T18:42:12Z

modules/rgbd/src/pose_graph.cpp

+        Vec6f delta          = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1));
+        Affine3f pose        = nodes.at(currentNodeId).getPose();
+        Affine3f currDelta   = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3));
+        Affine3f updatedPose = currDelta * pose;


Be careful with Affine3f multiplication (here and everywhere else), looks like they are multiplied in reverse order compared to pose matrices.

- Minor changes to Gauss newton and Sparse matrix. Residual still increases slightly over iterations

savuor · 2020-09-01T21:10:52Z

modules/rgbd/src/large_kinfu.cpp

+        p.volumeParams.tsdfTruncDist       = 7 * p.volumeParams.voxelSize;  // about 0.04f in meters
+        p.volumeParams.maxWeight           = 64;                            // frames
+        p.volumeParams.raycastStepFactor   = 0.25f;                         // in voxel sizes
+        p.volumeParams.depthTruncThreshold = p.truncateThreshold;


Does this thing duplicate the params in volume.cpp?

Yes, pretty much. Since there is no copy constructor for volumeParams, this is done.

savuor · 2020-09-21T15:44:56Z

modules/rgbd/src/large_kinfu.cpp

+        // TODO: Convert constraints to posegraph
+        PoseGraph poseGraph = submapMgr->MapToPoseGraph();
+        std::cout << "Created posegraph\n";
+        Optimizer::optimizeCeres(poseGraph);


Please change the name to optimize(), i.e. w/o mentioning a backend's name. This can be changed later.

into submap

…o submap

akashsharma02 · 2020-09-22T20:31:17Z

Recent PR merge #2629 is possibly causing a problem with tracking in the demo (kinfu_demo and large_kinfu_demo). Will need to debug more.

savuor · 2020-09-26T16:25:49Z

The fix for the PR #2629 is going to be delivered soon. As soon as it's done, this PR should be build-fixed and merged too.
Another thing to be changed before the merge is CRTP for HashTSDF. I think that it complicates the code too much and gives no significant benefit in performance.
The rest things (like moving from Ceres or accumulating poses w/o SE(3) matrices averaging) should be done later, in the nearest future.

savuor · 2020-10-05T12:12:48Z

@akashsharma02 will you fix this or should I do that?
(Akash Sharma responded: will fix this by the end of this week)

…o submap

savuor · 2020-10-12T01:01:02Z

@akashsharma02 when you're ready, please rename this PR: remove [WIP] part

savuor · 2020-10-12T19:05:45Z

👍

NikolausDemmel · 2020-10-29T00:44:40Z

The build will break with the new version of Ceres, 2.0.0, which just got released. Main issue probably is that it now requires C++14. Same issue as with the sfm module (see #2578). Maybe someone could check if it's possible to use C++14 mode for at least the files that depend on ceres, if ceres version >= 2.0.0 was found?

savuor · 2020-10-29T14:17:57Z

The true solution should be to get rid of Ceres in this module.
Ceres used in error function and its Jacobian estimation only; the corresponding pieces of code are small enough to be borrowed from Ceres or rewritten from scratch.
We had that thing planned but have no time to do this yet.
The Ceres solver itself is not used, we build the sparse matrix and solve it using Eigen.

NikolausDemmel · 2020-11-07T16:08:20Z

Thanks for the response. Makes sense.

In the meantime, looks like the immediate issue regarding compilation with ceres 2.0 got fixed in #2732.

PeterBowman · 2020-11-16T15:23:00Z

Hi. cv::dynafu::Params has been removed in favor of cv::kinfu::Params at aea037a. Perhaps the former should be typedefed in order to avoid breaking compat with CV 4.2-4.5.

Edit: done at f639657, thanks!

akashsharma02 and others added 21 commits March 21, 2020 20:32

Merge pull request #1 from opencv/master

4dbdee4

Merge opencv master

Merge remote-tracking branch 'upstream/master'

dcbe5a8

Merge remote-tracking branch 'upstream/master'

a7c3ab0

- Add HashTSDF class

3fa8a3c

- Implement Integrate function (untested)

Integration seems to be working, raycasting does not

98ce3d0

Update integration code

01a172c

Integration and Raycasting fixes, (both work now)

5a3f537

- Format code

9b5b0ce

- Clean up comments and few fixes

Add Kinect Fusion backup file

bdbafd0

- Add interpolation for vertices and normals (slow and unreliable!)

70b1dfb

- Format code - Delete kinfu_back.cpp

Bug fix for integration and noisy odometry

fcfd6dd

- Create volume abstract class

ddf5c18

- Address Review comments

- Add getPoints and getNormals function

a00d7eb

- Fix formatting according to comments - Move volume abstract class to include/opencv2/rgbd/ - Write factory method for creating TSDFVolumes - Small bug fixes - Minor fixes according to comments

- Add tests for hashTSDF

75656bd

- Fix raycasting bug causing to loop forever - Suppress warnings by explicit conversion - Disable hashTsdf test until we figure out memory leak - style changes - Add missing license in a few files, correct precomp.hpp usage

Merge branch 'master' of https://github.com/opencv/opencv_contrib

776c8f8

- Use CRTP based static polymorphism to choose between CPU and GPU for

d5ccfde

HashTSDF volume

Create submap and submapMgr

8eaa125

Implement overlap_ratio check to create new submaps

Early draft of posegraph and submaps (Doesn't even compile)

362ec1a

Minor cleanup (no compilation)

7264be2

Track all submaps (no posegraph update yet)

a96cf66

Return inliers from ICP for weighting the constraints

02fd312

(Huber threshold based inliers pending)

savuor self-assigned this Jul 30, 2020

akashsharma02 added 5 commits August 1, 2020 12:06

Add updating constraints between submaps and retain same current map

034970a

Fix constraints creation between submaps and allow for switching between

8553212

submaps

- Fix bug in allocate volumeUnits

05db54b

- Simplify calculation of visibleBlocks

Remove inlier calculation in fast_icp (not required)

5085884

Modify readFile to allow reading other datasets easily

de7ba96

savuor reviewed Aug 6, 2020

View reviewed changes

- Implement posegraph update, Gauss newton is unstable

963b086

- Minor changes to Gauss newton and Sparse matrix. Residual still increases slightly over iterations

akashsharma02 force-pushed the submap branch from f543422 to 963b086 Compare August 8, 2020 22:28

savuor reviewed Sep 1, 2020

View reviewed changes

savuor reviewed Sep 21, 2020

View reviewed changes

akashsharma02 added 3 commits September 22, 2020 15:31

Merge branch 'master' of https://github.com/akashsharma02/opencv_contrib

fe01b57

into submap

Minor API changes

9599c3a

Merge branch 'master' of https://github.com/opencv/opencv_contrib int…

b5051a4

…o submap

Minor

dfce899

akashsharma02 added 2 commits October 10, 2020 19:14

Merge branch 'master' of https://github.com/opencv/opencv_contrib int…

8f82ccc

…o submap

Remove CRTP for HashTSDF class

48ee51e

akashsharma02 force-pushed the submap branch from 9b148b2 to 48ee51e Compare October 10, 2020 23:40

Bug fixes for HashTSDF integration

daa800f

akashsharma02 force-pushed the submap branch from f329273 to daa800f Compare October 12, 2020 01:13

akashsharma02 changed the title ~~[GSoC] [WIP] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion~~ [GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion Oct 12, 2020

savuor self-requested a review October 12, 2020 19:03

savuor approved these changes Oct 12, 2020

View reviewed changes

alalek merged commit 7022f4e into opencv:master Oct 13, 2020

alalek mentioned this pull request Oct 28, 2020

Fix build failure in rgbd's test #2730

Merged

6 tasks

NikolausDemmel mentioned this pull request Oct 29, 2020

Ceres 2.0.0 upgrade to C++14 breaks sfm module #2578

Closed

4 tasks

NikolausDemmel mentioned this pull request Oct 31, 2020

ceres-solver 2.0.0 Homebrew/homebrew-core#63596

Closed

akashsharma02 deleted the submap branch November 16, 2020 16:43

PeterBowman mentioned this pull request Nov 16, 2020

Introducing Kinect Fusion as a service roboticslab-uc3m/vision#101

Closed

alalek mentioned this pull request Nov 27, 2020

Merge 4.x #2768

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion #2619

[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion #2619

akashsharma02 commented Jul 29, 2020 •

edited

Loading

savuor Aug 5, 2020

akashsharma02 Aug 27, 2020

savuor Aug 5, 2020

akashsharma02 Aug 27, 2020

savuor Aug 5, 2020

akashsharma02 Aug 7, 2020

savuor Aug 7, 2020

akashsharma02 Aug 27, 2020

savuor Aug 28, 2020

savuor Aug 5, 2020

akashsharma02 Aug 27, 2020

savuor Aug 5, 2020

akashsharma02 Aug 27, 2020

savuor Aug 27, 2020

savuor Aug 6, 2020

akashsharma02 Aug 7, 2020

savuor Aug 6, 2020

savuor Aug 6, 2020

savuor Aug 6, 2020

akashsharma02 Aug 7, 2020

savuor Aug 6, 2020

savuor Sep 1, 2020

akashsharma02 Sep 2, 2020 •

edited

Loading

savuor Sep 21, 2020

akashsharma02 Sep 22, 2020

akashsharma02 commented Sep 22, 2020

savuor commented Sep 26, 2020

savuor commented Oct 5, 2020 •

edited

Loading

savuor commented Oct 12, 2020

savuor commented Oct 12, 2020

NikolausDemmel commented Oct 29, 2020

savuor commented Oct 29, 2020

NikolausDemmel commented Nov 7, 2020

PeterBowman commented Nov 16, 2020 •

edited

Loading


		CV_WRAP virtual void reset() = 0;

		virtual const Affine3f getPose() const = 0;


		PoseConstraint() : weight(0){};

		void accumulatePose(const Affine3f& _pose, int _weight = 1)

[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion #2619

[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion #2619

Conversation

akashsharma02 commented Jul 29, 2020 • edited Loading

Checklist before review:

Pull Request Readiness Checklist

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

akashsharma02 Sep 2, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

akashsharma02 commented Sep 22, 2020

savuor commented Sep 26, 2020

savuor commented Oct 5, 2020 • edited Loading

savuor commented Oct 12, 2020

savuor commented Oct 12, 2020

NikolausDemmel commented Oct 29, 2020

savuor commented Oct 29, 2020

NikolausDemmel commented Nov 7, 2020

PeterBowman commented Nov 16, 2020 • edited Loading

akashsharma02 commented Jul 29, 2020 •

edited

Loading

akashsharma02 Sep 2, 2020 •

edited

Loading

savuor commented Oct 5, 2020 •

edited

Loading

PeterBowman commented Nov 16, 2020 •

edited

Loading