- Implement posegraph update, Gauss newton is unstable

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

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,51 @@ 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 << "Edge estimated transformation: \n" << currEdge.transformation.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: \n" << 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 = std::sqrt((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;
 }
 
@@ -161,6 +187,7 @@ PoseGraph PoseGraph::update(const Mat& deltaVec)
         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));
+        std::cout << "Current Delta for node ID: " << currentNodeId << " \n" << currDelta.matrix << std::endl;
         Affine3f updatedPose = currDelta * pose;
 
         updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose);
@@ -169,6 +196,20 @@ PoseGraph PoseGraph::update(const Mat& deltaVec)
     return updatedPoseGraph;
 }
 
+Mat PoseGraph::getVector()
+{
+    int numNodes = int(nodes.size());
+    Mat vector(6 * numNodes, 1, CV_32F, Scalar(0));
+    for(int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++)
+    {
+        Affine3f pose = nodes.at(currentNodeId).getPose();
+        Vec3f rot = pose.rvec();
+        Vec3f trans = pose.translation();
+        vector.rowRange(6 * currentNodeId, 6 * (currentNodeId+1)) = Vec6f(rot.val[0], rot.val[1], rot.val[2], trans.val[0], trans.val[1], trans.val[2]);
+    }
+    return vector;
+}
+
 //! NOTE: We follow left-composition for the infinitesimal pose update
 void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph)
 {
@@ -187,38 +228,49 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph&
     std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl;
 
     int iter = 0;
-    //! converged is set to true when error/residual is within some limit
-    bool converged     = false;
     float prevResidual = std::numeric_limits<float>::max();
     do
     {
+        std::cout << "Current iteration: " << iter << std::endl;
         //! 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);
+        Mat X = poseGraph.getVector();
         bool success = sparseSolve(ApproxH, B, delta);
         if (!success)
         {
             CV_Error(Error::StsNoConv, "Sparse solve failed");
             return;
         }
+        std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) << std::endl;
+
         //! Check delta
+        if(cv::norm(delta) < 1e-6f * (cv::norm(X) + 1e-6f))
+        {
+            std::cout << "Delta norm[" << cv::norm(delta) << "] < 1e-6f * X norm[" << cv::norm(X) <<"] + 1e-6f\n";
+            break;
+        }
+        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)
-            converged = true;
-
+        }
+        //! If updates don't improve a lot means loss function basin has been reached
+        if ((currentResidual - params.minResidual) < 1e-5f)
+        {
+            std::cout << "Gauss newton converged \n";
+            break;
+        }
         poseGraph = poseGraph.update(delta);
-        std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n";
-
         prevResidual = currentResidual;
-
-    } while (iter < params.maxNumIters && !converged);
+    } while (iter < params.maxNumIters);
 }
 }  // namespace kinfu
 }  // namespace cv

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; }
@@ -64,7 +64,8 @@ struct PoseGraphEdge
 //! Jose Luis Blanco
 //! Compactly represents the jacobian of the SE3 generator
 // clang-format off
-static const std::array<Matx44f, 6> generatorJacobian = {  // alpha
+static const std::array<Matx44f, 6> generatorJacobian = {
+    // alpha
     Matx44f(0, 0,  0, 0,
             0, 0, -1, 0,
             0, 1,  0, 0,
@@ -126,11 +127,12 @@ class PoseGraph
     int getNumNodes() const { return nodes.size(); }
     int getNumEdges() const { return edges.size(); }
 
+    Mat getVector();
+
     //! @brief: Constructs a linear system and returns the residual of the current system
     float createLinearSystem(BlockSparseMat<float, 6, 6>& H, Mat& B);
 
    private:
-
     NodeVector nodes;
     EdgeVector edges;
 };
@@ -144,7 +146,7 @@ struct Params
     float maxAcceptableResIncre;
 
     // TODO: Refine these constants
-    Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f){};
+    Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-2f){};
     virtual ~Params() = default;
 };