Implement posegraph update, Gauss newton is unstable

Author: akashsharma02

modules/rgbd/src/large_kinfu.cpp

@@ -227,7 +227,6 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
     std::cout << "Current frameID: " << frameCounter << "\n";
     for (const auto& it : submapMgr->activeSubmaps)
     {
-        //! Iterate over map?
         int currTrackingId = it.first;
         auto submapData = it.second;
         Ptr<Submap<MatType>> currTrackingSubmap = submapMgr->getSubmap(currTrackingId);
@@ -250,6 +249,7 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
         else
         {
             std::cout << "Tracking failed" << std::endl;
+            continue;
         }
 
         //2. Integrate
@@ -272,7 +272,17 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
 
     }
     //4. Update map
-    submapMgr->updateMap(frameCounter, newPoints, newNormals);
+    bool isMapUpdated = submapMgr->updateMap(frameCounter, newPoints, newNormals);
+
+    if(isMapUpdated)
+    {
+        // TODO: Convert constraints to posegraph
+        PoseGraph poseGraph = submapMgr->createPoseGraph();
+        std::cout << "Created posegraph\n";
+        Optimizer::optimizeGaussNewton(Optimizer::Params(), poseGraph);
+        /* submapMgr->adjustMap(poseGraph); */
+
+    }
     std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n";
 
     frameCounter++;

modules/rgbd/src/pose_graph.cpp

@@ -15,7 +15,6 @@ namespace cv
 {
 namespace kinfu
 {
-
 bool PoseGraph::isValid() const
 {
     int numNodes = int(nodes.size());
@@ -34,7 +33,7 @@ bool PoseGraph::isValid() const
     {
         int currNodeId = nodesToVisit.back();
         nodesToVisit.pop_back();
-
+        std::cout << "Visiting node: " << currNodeId << "\n";
         // Since each node does not maintain its neighbor list
         for (int i = 0; i < numEdges; i++)
         {
@@ -49,20 +48,20 @@ bool PoseGraph::isValid() const
             {
                 nextNodeId = potentialEdge.getSourceNodeId();
             }
-
+            std::cout << "Next node: " << nextNodeId << std::endl;
             if (nextNodeId != -1)
             {
-                if(nodesVisited.count(currNodeId) == 0)
+                if (nodesVisited.count(currNodeId) == 0)
                 {
                     nodesVisited.insert(currNodeId);
+                    nodesToVisit.push_back(nextNodeId);
                 }
-                nodesToVisit.push_back(nextNodeId);
             }
         }
     }
 
     isGraphConnected = (int(nodesVisited.size()) == numNodes);
-
+    std::cout << "IsGraphConnected: " << isGraphConnected << std::endl;
     bool invalidEdgeNode = false;
     for (int i = 0; i < numEdges; i++)
     {
@@ -81,26 +80,37 @@ float PoseGraph::createLinearSystem(BlockSparseMat<float, 6, 6>& H, Mat& B)
 {
     int numEdges = int(edges.size());
 
-    float residual = 0;
+    float residual = 0.0f;
 
-    //! Lmabda to calculate error jacobian for a particular constraint
+    // Lambda to calculate the error Jacobians w.r.t source and target poses
     auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas,
-                                const Affine3f& sourcePose, const Affine3f& targetPose) -> void {
+                                      const Affine3f& sourcePose, const Affine3f& targetPose) -> void
+    {
         const Affine3f relativePoseMeas_inv = relativePoseMeas.inv();
         const Affine3f targetPose_inv       = targetPose.inv();
         for (int i = 0; i < 6; i++)
         {
             Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose;
-            Vec3f rot               = dError_dSource.rvec();
-            Vec3f trans             = dError_dSource.translation();
-            jacSource.col(i)        = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2));
+            Vec3f rot   = dError_dSource.rvec();
+            Vec3f trans = dError_dSource.translation();
+            jacSource.val[i + 6*0] = rot(0);
+            jacSource.val[i + 6*1] = rot(1);
+            jacSource.val[i + 6*2] = rot(2);
+            jacSource.val[i + 6*3] = trans(0);
+            jacSource.val[i + 6*4] = trans(1);
+            jacSource.val[i + 6*5] = trans(2);
         }
         for (int i = 0; i < 6; i++)
         {
             Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose;
-            Vec3f rot               = dError_dSource.rvec();
-            Vec3f trans             = dError_dSource.translation();
-            jacTarget.col(i)        = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2));
+            Vec3f rot   = dError_dSource.rvec();
+            Vec3f trans = dError_dSource.translation();
+            jacTarget.val[i + 6*0] = rot(0);
+            jacTarget.val[i + 6*1] = rot(1);
+            jacTarget.val[i + 6*2] = rot(2);
+            jacTarget.val[i + 6*3] = trans(0);
+            jacTarget.val[i + 6*4] = trans(1);
+            jacTarget.val[i + 6*5] = trans(2);
         }
     };
 
@@ -117,35 +127,50 @@ float PoseGraph::createLinearSystem(BlockSparseMat<float, 6, 6>& H, Mat& B)
         //! Edge transformation is source to target. (relativeConstraint should be identity if no error)
         Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget;
 
-        Vec6f error;
+        std::cout << "RelativeSourceToTarget: \n" << relativeSourceToTarget.matrix << "\n";
+        std::cout << "PoseConstraint: \n" << poseConstraint.matrix << "\n";
+
+        Matx61f error;
         Vec3f rot   = poseConstraint.rvec();
         Vec3f trans = poseConstraint.translation();
 
-        error[0] = rot[0];
-        error[1] = rot[1];
-        error[2] = rot[2];
-        error[3] = trans[0];
-        error[4] = trans[1];
-        error[5] = trans[2];
+        error.val[0] = rot[0];
+        error.val[1] = rot[1];
+        error.val[2] = rot[2];
+        error.val[3] = trans[0];
+        error.val[4] = trans[1];
+        error.val[5] = trans[2];
+
+        std::cout << "Error vector: " << error << std::endl;
 
         Matx66f jacSource, jacTarget;
         calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose);
+        std::cout << "Source Jacobian: \n" << jacSource << std::endl;
+        std::cout << "Target Jacobian: \n" << jacTarget << std::endl;
 
         Matx16f errorTransposeInfo     = error.t() * currEdge.information;
         Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information;
         Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information;
 
-        residual += (errorTransposeInfo * error)[0];
+        std::cout << "errorTransposeInfo: " << errorTransposeInfo << "\n";
+        std::cout << "jacSourceTransposeInfo: " << jacSourceTransposeInfo << "\n";
+        std::cout << "jacTargetTransposeInfo: " << jacTargetTransposeInfo << "\n";
+
+        float res = (errorTransposeInfo * error).val[0];
+        residual += res;
 
+        std::cout << "sourceNodeId: " << sourceNodeId << " targetNodeId: " << targetNodeId << std::endl;
         H.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource;
         H.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget;
         H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget;
         H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource;
 
-        B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += errorTransposeInfo * jacSource;
-        B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += errorTransposeInfo * jacTarget;
+
+        B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += (errorTransposeInfo * jacSource).reshape<6, 1>();
+        B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += (errorTransposeInfo * jacTarget).reshape<6, 1>();
     }
 
+    std::cout << "Residual value: " << residual << std::endl;
     return residual;
 }
 
@@ -194,27 +219,34 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph&
     {
         //! ApproxH = Approximate Hessian = J^T * information * J
         BlockSparseMat<float, 6, 6> ApproxH(numNodes);
-        Mat B(6 * numNodes, 1, CV_32F);
+        Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F);
 
         float currentResidual = poseGraph.createLinearSystem(ApproxH, B);
-
+        std::cout << "Number of non-zero blocks in H: " << ApproxH.nonZeroBlocks() << "\n";
         Mat delta(6 * numNodes, 1, CV_32F);
         bool success = sparseSolve(ApproxH, B, delta);
         if (!success)
         {
             CV_Error(Error::StsNoConv, "Sparse solve failed");
             return;
         }
+        std::cout << "delta update: " << delta << "\n";
         //! Check delta
+        std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n";
 
         //! TODO: Improve criterion and allow small increments in residual
-        if((currentResidual - prevResidual) > params.maxAcceptableResIncre)
+        if ((currentResidual - prevResidual) > params.maxAcceptableResIncre)
+        {
+            std::cout << "Current residual increased from prevResidual by at least " << params.maxAcceptableResIncre << std::endl;
             break;
+        }
         if ((currentResidual - params.minResidual) < 0.00001f)
+        {
+            std::cout << "Gauss newton converged \n";
             converged = true;
+        }
 
         poseGraph = poseGraph.update(delta);
-        std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n";
 
         prevResidual = currentResidual;
 

modules/rgbd/src/pose_graph.hpp

@@ -18,8 +18,8 @@ namespace kinfu
 struct PoseGraphNode
 {
    public:
-    explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose)  {}
-    virtual ~PoseGraphNode();
+    explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) {}
+    virtual ~PoseGraphNode() = default;
 
     int getId() const { return nodeId; }
     Affine3f getPose() const { return pose; }
@@ -130,7 +130,6 @@ class PoseGraph
     float createLinearSystem(BlockSparseMat<float, 6, 6>& H, Mat& B);
 
    private:
-
     NodeVector nodes;
     EdgeVector edges;
 };

modules/rgbd/src/sparse_block_matrix.hpp

@@ -50,7 +50,7 @@ struct BlockSparseMat
         auto it = ijValue.find(p);
         if (it == ijValue.end())
         {
-            it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>() }).first;
+            it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>::zeros() }).first;
         }
         return it->second;
     }
@@ -65,25 +65,29 @@ struct BlockSparseMat
     Eigen::SparseMatrix<_Tp> toEigen() const
     {
         std::vector<Eigen::Triplet<double>> tripletList;
-        tripletList.reserve(ijValue.size() * blockSize * blockSize);
-
+        tripletList.reserve(ijValue.size() * blockSize);
         for (auto ijv : ijValue)
         {
             int xb = ijv.first.x, yb = ijv.first.y;
+            std::cout << "xb: " << xb << " yb: " << yb << "\n";
             MatType vblock = ijv.second;
-            for (int i = 0; i < blockSize; i++)
+            std::cout << "vblock: " << vblock << "\n";
+            for (int i = 0; i < blockM; i++)
             {
-                for (int j = 0; j < blockSize; j++)
+                for (int j = 0; j < blockN; j++)
                 {
                     float val = vblock(i, j);
                     if (abs(val) >= NON_ZERO_VAL_THRESHOLD)
                     {
-                        tripletList.push_back(Eigen::Triplet<double>(blockSize * xb + i, blockSize * yb + j, val));
+                        std::cout << "Pushing back triplet: (" << i  << ", " << j << ", " << val << ")\n";
+                        tripletList.push_back(Eigen::Triplet<double>(blockM * xb + i, blockN * yb + j, val));
                     }
                 }
             }
         }
-        Eigen::SparseMatrix<_Tp> EigenMat(blockSize * nBlocks, blockSize * nBlocks);
+        std::cout << "tripletListSize: " << tripletList.size() << "\n";
+        std::cout << blockM << " " << nBlocks << " " << blockN << std::endl;
+        Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks);
         EigenMat.setFromTriplets(tripletList.begin(), tripletList.end());
         EigenMat.makeCompressed();
 
@@ -118,13 +122,14 @@ static bool sparseSolve(const BlockSparseMat<float, 6, 6>& H, const Mat& B, Mat&
         return result;
     }
 
-
     // TODO: try this, LLT and Cholesky
     Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;
 
     std::cout << "starting eigen-compute..." << std::endl;
     solver.compute(bigA);
 
+    std::cout << "solver info: " << solver.info() << "\n";
+
     if (solver.info() != Eigen::Success)
     {
         std::cout << "failed to eigen-decompose" << std::endl;