From 9ff635ea5aab584fc04b01f7cb0d581821167b37 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Mar 2021 01:27:39 +0300 Subject: [PATCH 01/51] Works well on g2o dataset, cumulative: 1. Pose3d done w/o Eigen types; 2. PoseGraph nodes vector -> map 3. Eigen is not used in cost function or parametrization 4. Cost function debugged & fixed (original one was wrong), rewritten from Automatic to Analytic --- modules/rgbd/src/pose_graph.cpp | 112 ++++++++-- modules/rgbd/src/pose_graph.hpp | 380 ++++++++++++++++++++++++++------ 2 files changed, 399 insertions(+), 93 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index b525e455129..b17cf4c5671 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -28,14 +28,14 @@ bool PoseGraph::isValid() const std::unordered_set nodesVisited; std::vector nodesToVisit; - nodesToVisit.push_back(nodes.at(0).getId()); + nodesToVisit.push_back(nodes.begin()->first); bool isGraphConnected = false; while (!nodesToVisit.empty()) { int currNodeId = nodesToVisit.back(); nodesToVisit.pop_back(); - std::cout << "Visiting node: " << currNodeId << "\n"; + //std::cout << "Visiting node: " << currNodeId << "\n"; nodesVisited.insert(currNodeId); // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) @@ -53,8 +53,8 @@ bool PoseGraph::isValid() const } if (nextNodeId != -1) { - std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) - << std::endl; + //std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) + // << std::endl; if (nodesVisited.count(nextNodeId) == 0) { nodesToVisit.push_back(nextNodeId); @@ -82,10 +82,62 @@ bool PoseGraph::isValid() const } #if defined(CERES_FOUND) && defined(HAVE_EIGEN) + +class MyQuaternionParameterization + : public ceres::LocalParameterization { +public: + virtual ~MyQuaternionParameterization() {} + bool Plus(const double* x_ptr, const double* delta_ptr, double* x_plus_delta_ptr) const override + { + Vec4d vx(x_ptr); + Quatd x(vx); + Vec3d delta(delta_ptr); + + const double norm_delta = norm(delta); + Quatd x_plus_delta; + if (norm_delta > 0.0) + { + const double sin_delta_by_delta = std::sin(norm_delta) / norm_delta; + + // Note, in the constructor w is first. + Quatd delta_q(std::cos(norm_delta), + sin_delta_by_delta * delta[0], + sin_delta_by_delta * delta[1], + sin_delta_by_delta * delta[2]); + x_plus_delta = delta_q * x; + } + else + { + x_plus_delta = x; + } + + Vec4d xpd = x_plus_delta.toVec(); + x_plus_delta_ptr[0] = xpd[0]; + x_plus_delta_ptr[1] = xpd[1]; + x_plus_delta_ptr[2] = xpd[2]; + x_plus_delta_ptr[3] = xpd[3]; + + return true; + } + + bool ComputeJacobian(const double* x, double* jacobian) const override + { + // clang-format off + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; + // clang-format on + return true; + } + + int GlobalSize() const override { return 4; } + int LocalSize() const override { return 3; } +}; + void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) { int numEdges = poseGraph.getNumEdges(); - int numNodes = poseGraph.getNumNodes(); if (numEdges == 0) { CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); @@ -97,34 +149,48 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& ceres::LocalParameterization* quatLocalParameterization = new ceres::EigenQuaternionParameterization; - for (int currEdgeNum = 0; currEdgeNum < numEdges; ++currEdgeNum) + for (const PoseGraphEdge& currEdge : poseGraph.edges) { - const PoseGraphEdge& currEdge = poseGraph.edges.at(currEdgeNum); - int sourceNodeId = currEdge.getSourceNodeId(); - int targetNodeId = currEdge.getTargetNodeId(); - Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; - Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; + int sourceNodeId = currEdge.getSourceNodeId(); + int targetNodeId = currEdge.getTargetNodeId(); + Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; + Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; + + // ------- - const Matx66f& informationMatrix = currEdge.information; + Eigen::Matrix info; + cv2eigen(Matx66d(currEdge.information), info); + const Eigen::Matrix sqrt_information = info.llt().matrixL(); + Matx66d sqrtInfo; + eigen2cv(sqrt_information, sqrtInfo); ceres::CostFunction* costFunction = Pose3dErrorFunctor::create( Pose3d(currEdge.transformation.rotation(), currEdge.transformation.translation()), - informationMatrix); + sqrtInfo); + + // ------- + + ceres::CostFunction* costFunction2 = Pose3dAnalyticCostFunction::create( + Vec3d(currEdge.transformation.translation()), + Quatd::createFromRotMat(Matx33d(currEdge.transformation.rotation())), + currEdge.information); + + // ------- - problem.AddResidualBlock(costFunction, lossFunction, sourcePose.t.data(), - sourcePose.r.coeffs().data(), targetPose.t.data(), - targetPose.r.coeffs().data()); - problem.SetParameterization(sourcePose.r.coeffs().data(), quatLocalParameterization); - problem.SetParameterization(targetPose.r.coeffs().data(), quatLocalParameterization); + problem.AddResidualBlock(costFunction2, lossFunction, + sourcePose.t.val, sourcePose.vq.val, + targetPose.t.val, targetPose.vq.val); + problem.SetParameterization(sourcePose.vq.val, quatLocalParameterization); + problem.SetParameterization(targetPose.vq.val, quatLocalParameterization); } - for (int currNodeId = 0; currNodeId < numNodes; ++currNodeId) + for (const auto& it : poseGraph.nodes) { - PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); - if (currNode.isPoseFixed()) + const PoseGraphNode& node = it.second; + if (node.isPoseFixed()) { - problem.SetParameterBlockConstant(currNode.se3Pose.t.data()); - problem.SetParameterBlockConstant(currNode.se3Pose.r.coeffs().data()); + problem.SetParameterBlockConstant(node.se3Pose.t.val); + problem.SetParameterBlockConstant(node.se3Pose.vq.val); } } } diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index e8b7c34c53b..8f8429c01d6 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -15,6 +15,10 @@ #include #endif +#include "sparse_block_matrix.hpp" + +#include "opencv2/core/dualquaternion.hpp" + namespace cv { namespace kinfu @@ -24,76 +28,51 @@ namespace kinfu * * Detailed description */ -#if defined(HAVE_EIGEN) + struct Pose3d { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Eigen::Vector3d t; - Eigen::Quaterniond r; + Vec3d t; + Vec4d vq; - Pose3d() - { - t.setZero(); - r.setIdentity(); - }; - Pose3d(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) - : t(translation), r(Eigen::Quaterniond(rotation)) - { - normalizeRotation(); - } + Pose3d() : t(), vq(1, 0, 0, 0) { } Pose3d(const Matx33d& rotation, const Vec3d& translation) + : t(translation) { - Eigen::Matrix3d R; - cv2eigen(rotation, R); - cv2eigen(translation, t); - r = Eigen::Quaterniond(R); - normalizeRotation(); + vq = Quatd::createFromRotMat(rotation).normalize().toVec(); } - explicit Pose3d(const Matx44f& pose) - { - Matx33d rotation(pose.val[0], pose.val[1], pose.val[2], pose.val[4], pose.val[5], - pose.val[6], pose.val[8], pose.val[9], pose.val[10]); - Vec3d translation(pose.val[3], pose.val[7], pose.val[11]); - Pose3d(rotation, translation); - } + explicit Pose3d(const Matx44d& pose) : + Pose3d(pose.get_minor<3, 3>(0, 0), Vec3d(pose(0, 3), pose(1, 3), pose(2, 3))) + { } // NOTE: Eigen overloads quaternion multiplication appropriately inline Pose3d operator*(const Pose3d& otherPose) const { Pose3d out(*this); - out.t += r * otherPose.t; - out.r *= otherPose.r; - out.normalizeRotation(); + out.t += Quatd(vq).toRotMat3x3() * otherPose.t; + out.vq = (Quatd(out.vq) * Quatd(otherPose.vq)).toVec(); return out; } - inline Pose3d& operator*=(const Pose3d& otherPose) + Affine3d getAffine() const { - t += otherPose.t; - r *= otherPose.r; - normalizeRotation(); - return *this; + return Affine3d(Quatd(vq).toRotMat3x3(), t); } inline Pose3d inverse() const { Pose3d out; - out.r = r.conjugate(); - out.t = out.r * (t * -1.0); + out.vq = Quatd(vq).conjugate().toVec(); + out.t = - (Quatd(out.vq).toRotMat3x3(QUAT_ASSUME_UNIT) * t); return out; } inline void normalizeRotation() { - if (r.w() < 0) - r.coeffs() *= -1.0; - r.normalize(); + vq = Quatd(vq).normalize().toVec(); } }; -#endif struct PoseGraphNode { @@ -101,9 +80,7 @@ struct PoseGraphNode explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) { -#if defined(HAVE_EIGEN) se3Pose = Pose3d(_pose.rotation(), _pose.translation()); -#endif } virtual ~PoseGraphNode() = default; @@ -115,24 +92,13 @@ struct PoseGraphNode void setPose(const Affine3f& _pose) { pose = _pose; -#if defined(HAVE_EIGEN) se3Pose = Pose3d(pose.rotation(), pose.translation()); -#endif } -#if defined(HAVE_EIGEN) void setPose(const Pose3d& _pose) { se3Pose = _pose; - const Eigen::Matrix3d& rotation = se3Pose.r.toRotationMatrix(); - const Eigen::Vector3d& translation = se3Pose.t; - Matx33d rot; - Vec3d trans; - eigen2cv(rotation, rot); - eigen2cv(translation, trans); - Affine3d poseMatrix(rot, trans); - pose = poseMatrix; + pose = Affine3d(Quatd(se3Pose.vq).toRotMat3x3(QUAT_ASSUME_UNIT), se3Pose.t); } -#endif void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } @@ -140,9 +106,7 @@ struct PoseGraphNode int nodeId; bool isFixed; Affine3f pose; -#if defined(HAVE_EIGEN) Pose3d se3Pose; -#endif }; /*! \class PoseGraphEdge @@ -221,24 +185,41 @@ struct PoseGraphEdge class PoseGraph { public: - typedef std::vector NodeVector; - typedef std::vector EdgeVector; + typedef std::map Nodes; + //typedef std::vector NodeVector; + typedef std::vector EdgeVector; - explicit PoseGraph(){}; + explicit PoseGraph() {}; virtual ~PoseGraph() = default; //! PoseGraph can be copied/cloned PoseGraph(const PoseGraph&) = default; PoseGraph& operator=(const PoseGraph&) = default; - void addNode(const PoseGraphNode& node) { nodes.push_back(node); } + void addNode(const PoseGraphNode& node) + { + int id = node.getId(); + const auto& it = nodes.find(id); + if (it != nodes.end()) + { + std::cout << "duplicated node, id=" << id << std::endl; + nodes.insert(it, { id, node }); + } + else + { + nodes.insert({ id, node }); + } + } void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } bool nodeExists(int nodeId) const { + return (nodes.find(nodeId) != nodes.end()); + /* return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { return currNode.getId() == nodeId; }) != nodes.end(); + */ } bool isValid() const; @@ -247,7 +228,7 @@ class PoseGraph int getNumEdges() const { return int(edges.size()); } public: - NodeVector nodes; + Nodes nodes; EdgeVector edges; }; @@ -280,20 +261,36 @@ class Pose3dErrorFunctor { Eigen::Map> sourceTrans(_pSourceTrans); Eigen::Map> targetTrans(_pTargetTrans); - Eigen::Map> sourceQuat(_pSourceQuat); - Eigen::Map> targetQuat(_pTargetQuat); + + //Eigen::Quaternion sourceQuat(_pSourceQuat[3], _pSourceQuat[0], _pSourceQuat[1], _pSourceQuat[2]); + //Eigen::Quaternion targetQuat(_pTargetQuat[3], _pTargetQuat[0], _pTargetQuat[1], _pTargetQuat[2]); + Eigen::Quaternion sourceQuat(_pSourceQuat[0], _pSourceQuat[1], _pSourceQuat[2], _pSourceQuat[3]); + Eigen::Quaternion targetQuat(_pTargetQuat[0], _pTargetQuat[1], _pTargetQuat[2], _pTargetQuat[3]); + Eigen::Map> residual(_pResidual); - Eigen::Quaternion targetQuatInv = targetQuat.conjugate(); + //Eigen::Quaternion targetQuatInv = targetQuat.conjugate(); + + //Eigen::Quaternion relativeQuat = targetQuatInv * sourceQuat; + //Eigen::Matrix relativeTrans = targetQuatInv * (targetTrans - sourceTrans); - Eigen::Quaternion relativeQuat = targetQuatInv * sourceQuat; - Eigen::Matrix relativeTrans = targetQuatInv * (targetTrans - sourceTrans); + Eigen::Quaternion sourceQuatInv = sourceQuat.conjugate(); + + Eigen::Quaternion relativeQuat = sourceQuatInv * targetQuat; + Eigen::Matrix relativeTrans = sourceQuatInv * (targetTrans - sourceTrans); //! Definition should actually be relativeQuat * poseMeasurement.r.conjugate() - Eigen::Quaternion deltaRot = - poseMeasurement.r.template cast() * relativeQuat.conjugate(); + Eigen::Quaternion pmr(T(poseMeasurement.vq[0]), T(poseMeasurement.vq[1]), T(poseMeasurement.vq[2]), T(poseMeasurement.vq[3])); + + //DEBUG + //Eigen::Quaternion deltaRot = pmr * relativeQuat.conjugate(); + Eigen::Quaternion deltaRot = relativeQuat.conjugate() * pmr; + + //DEBUG + //deltaRot = (deltaRot.w() < T(0)) ? deltaRot * Eigen::Quaternion(T(-1), T(0), T(0), T(0)) : deltaRot; - residual.template block<3, 1>(0, 0) = relativeTrans - poseMeasurement.t.template cast(); + Eigen::Matrix pmt(T(poseMeasurement.t[0]), T(poseMeasurement.t[1]), T(poseMeasurement.t[2])); + residual.template block<3, 1>(0, 0) = relativeTrans - pmt; residual.template block<3, 1>(3, 0) = T(2.0) * deltaRot.vec(); residual.applyOnTheLeft(sqrtInfo.template cast()); @@ -312,6 +309,249 @@ class Pose3dErrorFunctor const Pose3d poseMeasurement; Eigen::Matrix sqrtInfo; }; + +// matrix form of Im(a) +const Matx44d M_Im{ 0, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 }; + +// matrix form of conjugation +const Matx44d M_Conj{ 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0, + 0, 0, 0, -1 }; + +// matrix form of quaternion multiplication from left side +inline Matx44d m_left(Quatd q) +{ + // M_left(a)* V(b) = + // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 + // av | 0_3] + 0_3x1 | skew(av)]) * V(b) + + float w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, -z, y, + y, z, w, -x, + z, -y, x, w }; +} + +// matrix form of quaternion multiplication from right side +inline Matx44d m_right(Quatd q) +{ + // M_right(b)* V(a) = + // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 + // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) + + float w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, z, -y, + y, -z, w, x, + z, y, -x, w }; +} + +// concatenate matrices vertically +template static inline +Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) +{ + Matx<_Tp, m + k, n> res; + for (int i = 0; i < m; i++) + { + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } + } + for (int i = 0; i < k; i++) + { + for (int j = 0; j < n; j++) + { + res(m + i, j) = b(i, j); + } + } + return res; +} + +// concatenate matrices horizontally +template static inline +Matx<_Tp, m, n + k> concatHor(const Matx<_Tp, m, n>& a, const Matx<_Tp, m, k>& b) +{ + Matx<_Tp, m, n + k> res; + + for (int i = 0; i < m; i++) + { + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } + } + for (int i = 0; i < m; i++) + { + for (int j = 0; j < k; j++) + { + res(i, n + j) = b(i, j); + } + } + return res; +} + + +class Pose3dAnalyticCostFunction : public ceres::SizedCostFunction<6, 3, 4, 3, 4> +{ +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Pose3dAnalyticCostFunction(Vec3d _transMeasured, Quatd _rotMeasured, Matx66d _infoMatrix) : + transMeasured(_transMeasured), rotMeasured(_rotMeasured), infoMatrix(_infoMatrix), + sqrtInfoMatrix(llt6(infoMatrix)) + { } + + static Matx66d llt6(Matx66d m) + { + Matx66d L; + for (int i = 0; i < 6; i++) + { + for (int j = 0; j < (i + 1); j++) + { + double sum = 0; + for (int k = 0; k < j; k++) + sum += L(i, k) * L(j, k); + + if (i == j) + L(i, i) = sqrt(m(i, i) - sum); + else + L(i, j) = (1.0 / L(j, j) * (m(i, j) - sum)); + } + } + return L; + } + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override + { + Vec3d sourceTrans(parameters[0]); + Vec4d p1(parameters[1]); + Quatd sourceQuat(p1); + Vec3d targetTrans(parameters[2]); + Vec4d p3(parameters[3]); + Quatd targetQuat(p3); + + // err_r = 2*Im(conj(rel_r) * measure_r) = 2*Im(conj(target_r) * source_r * measure_r) + // err_t = conj(source_r) * (target_t - source_t) * source_r - measure_t + + Vec6d res; + + Quatd sourceQuatInv = sourceQuat.conjugate(); + Vec3d deltaTrans = targetTrans - sourceTrans; + + Quatd relativeQuat = sourceQuatInv * targetQuat; + Vec3d relativeTrans = sourceQuatInv.toRotMat3x3(cv::QUAT_ASSUME_UNIT) * deltaTrans; + + //! Definition should actually be relativeQuat * rotMeasured.conjugate() + Quatd deltaRot = relativeQuat.conjugate() * rotMeasured; + + Vec3d terr = relativeTrans - transMeasured; + Vec3d rerr = 2.0 * Vec3d(deltaRot.x, deltaRot.y, deltaRot.z); + Vec6d rterr(terr[0], terr[1], terr[2], rerr[0], rerr[1], rerr[2]); + + res = sqrtInfoMatrix * rterr; + + if (jacobians) + { + Matx stj, ttj; + Matx sqj, tqj; + + // d(err_r) = 2*Im(d(conj(target_r) * source_r * measure_r)) = < measure_r is constant > = + // 2*Im((conj(d(target_r)) * source_r + conj(target_r) * d(source_r)) * measure_r) + // d(target_r) == 0: + // # d(err_r) = 2*Im(conj(target_r) * d(source_r) * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) * V(d(source_r)) + // # d(err_r) / d(source_r) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) + Matx34d drdsq = 2.0 * (m_right(rotMeasured) * m_left(targetQuat.conjugate())).get_minor<3, 4>(1, 0); + + // d(source_r) == 0: + // # d(err_r) = 2*Im(conj(d(target_r)) * source_r * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj * V(d(target_r)) + // # d(err_r) / d(target_r) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj + Matx34d drdtq = 2.0 * (m_right(sourceQuat * rotMeasured) * M_Conj).get_minor<3, 4>(1, 0); + + // d(err_t) = d(conj(source_r) * (target_t - source_t) * source_r) = + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // conj(d(source_r)) * (target_t - source_t) * source_r + + // conj(source_r) * (target_t - source_t) * d(source_r) = + // + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // d(*_t) == 0: + // # d(err_t) = 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // # V(d(err_t)) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) * V(d(source_r)) + // # d(err_t) / d(source_r) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) + Vec3d td = targetTrans - sourceTrans; + Matx34d dtdsq = 2 * m_left(sourceQuatInv * Quatd(0, deltaTrans[0], deltaTrans[1], deltaTrans[2])).get_minor<3, 4>(1, 0); + // deltaTrans is rotated by sourceQuatInv, so the jacobian is rot matrix of sourceQuatInv by +1 or -1 + Matx33d dtdtt = sourceQuatInv.toRotMat3x3(QUAT_ASSUME_UNIT); + Matx33d dtdst = - dtdtt; + + Matx33d z; + sqj = concatVert(dtdsq, drdsq); + tqj = concatVert(Matx34d(), drdtq); + stj = concatVert(dtdst, z); + ttj = concatVert(dtdtt, z); + + stj = sqrtInfoMatrix * stj; + ttj = sqrtInfoMatrix * ttj; + sqj = sqrtInfoMatrix * sqj; + tqj = sqrtInfoMatrix * tqj; + + // sourceTrans + if (jacobians[0]) + { + for (int i = 0; i < 6 * 3; i++) + jacobians[0][i] = stj.val[i]; + } + + // sourceQuat + if (jacobians[1]) + { + for (int i = 0; i < 6 * 4; i++) + jacobians[1][i] = sqj.val[i]; + } + + // targetTrans + if (jacobians[2]) + { + for (int i = 0; i < 6 * 3; i++) + jacobians[2][i] = ttj.val[i]; + } + + // targetQuat + if (jacobians[3]) + { + for (int i = 0; i < 6 * 4; i++) + jacobians[3][i] = tqj.val[i]; + } + } + + for (int i = 0; i < 6; i++) + residuals[i] = res[i]; + + return true; + } + + static ceres::CostFunction* create(Vec3d _transMeasured, Quatd _rotMeasured, Matx66d _infoMatrix) + { + return new Pose3dAnalyticCostFunction(_transMeasured, _rotMeasured, _infoMatrix); + } + + Vec3d transMeasured; + Quatd rotMeasured; + Matx66d infoMatrix; + Matx66d sqrtInfoMatrix; +}; + + #endif } // namespace Optimizer From 69d05df5600159f7b7c929bd4c350db3f5d7719a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 10 Mar 2021 02:02:55 +0300 Subject: [PATCH 02/51] g2o dataset reading added to PoseGraph --- modules/rgbd/src/pose_graph.cpp | 92 +++++++++++++++++++++++++++++++++ modules/rgbd/src/pose_graph.hpp | 27 +++++----- 2 files changed, 107 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index b17cf4c5671..768c7b4548a 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -4,6 +4,7 @@ #include "pose_graph.hpp" +#include #include #include #include @@ -81,6 +82,97 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } + +// Taken from Ceres pose graph demo: https://ceres-solver.org/ +PoseGraph::PoseGraph(const std::string& g2oFileName) : + nodes(), edges() +{ + auto readAffine = [](std::istream& input) -> Affine3d + { + Vec3d p; + Vec4d q; + input >> p[0] >> p[1] >> p[2]; + input >> q[1] >> q[2] >> q[3] >> q[0]; + // Normalize the quaternion to account for precision loss due to + // serialization. + return Affine3d(Quatd(q).toRotMat3x3(), p); + }; + + // for debugging purposes + int minId = 0, maxId = 1 << 24; + + std::ifstream infile(g2oFileName.c_str()); + if (!infile) + { + CV_Error(cv::Error::StsError, "failed to open file"); + } + + while (infile.good()) + { + std::string data_type; + // Read whether the type is a node or a constraint + infile >> data_type; + if (data_type == "VERTEX_SE3:QUAT") + { + int id; + infile >> id; + Affine3d pose = readAffine(infile); + + if (id < minId || id >= maxId) + continue; + + kinfu::PoseGraphNode n(id, pose); + if (id == minId) + n.setFixed(); + + // Ensure we don't have duplicate poses + const auto& it = nodes.find(id); + if (it != nodes.end()) + { + std::cout << "duplicated node, id=" << id << std::endl; + nodes.insert(it, { id, n }); + } + else + { + nodes.insert({ id, n }); + } + } + else if (data_type == "EDGE_SE3:QUAT") + { + int startId, endId; + infile >> startId >> endId; + Affine3d pose = readAffine(infile); + + if (!((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId))) + continue; + + Matx66d info; + for (int i = 0; i < 6 && infile.good(); ++i) + { + for (int j = i; j < 6 && infile.good(); ++j) + { + infile >> info(i, j); + if (i != j) + { + info(j, i) = info(i, j); + } + } + } + + edges.push_back(PoseGraphEdge(startId, endId, pose, info)); + } + else + { + CV_Error(cv::Error::StsError, "unknown tag"); + } + + // Clear any trailing whitespace from the line + infile >> std::ws; + } +} + + + #if defined(CERES_FOUND) && defined(HAVE_EIGEN) class MyQuaternionParameterization diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 8f8429c01d6..963642ad776 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -50,14 +50,14 @@ struct Pose3d inline Pose3d operator*(const Pose3d& otherPose) const { Pose3d out(*this); - out.t += Quatd(vq).toRotMat3x3() * otherPose.t; + out.t += Quatd(vq).toRotMat3x3(QUAT_ASSUME_UNIT) * otherPose.t; out.vq = (Quatd(out.vq) * Quatd(otherPose.vq)).toVec(); return out; } Affine3d getAffine() const { - return Affine3d(Quatd(vq).toRotMat3x3(), t); + return Affine3d(Quatd(vq).toRotMat3x3(QUAT_ASSUME_UNIT), t); } inline Pose3d inverse() const @@ -77,7 +77,7 @@ struct Pose3d struct PoseGraphNode { public: - explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) + explicit PoseGraphNode(int _nodeId, const Affine3d& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) { se3Pose = Pose3d(_pose.rotation(), _pose.translation()); @@ -85,11 +85,12 @@ struct PoseGraphNode virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } - inline Affine3f getPose() const + inline Affine3d getPose() const { + pose = se3Pose.getAffine(); return pose; } - void setPose(const Affine3f& _pose) + void setPose(const Affine3d& _pose) { pose = _pose; se3Pose = Pose3d(pose.rotation(), pose.translation()); @@ -97,7 +98,7 @@ struct PoseGraphNode void setPose(const Pose3d& _pose) { se3Pose = _pose; - pose = Affine3d(Quatd(se3Pose.vq).toRotMat3x3(QUAT_ASSUME_UNIT), se3Pose.t); + pose = se3Pose.getAffine(); } void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } @@ -105,7 +106,7 @@ struct PoseGraphNode public: int nodeId; bool isFixed; - Affine3f pose; + mutable Affine3d pose; Pose3d se3Pose; }; @@ -185,9 +186,7 @@ struct PoseGraphEdge class PoseGraph { public: - typedef std::map Nodes; - //typedef std::vector NodeVector; - typedef std::vector EdgeVector; + explicit PoseGraph() {}; virtual ~PoseGraph() = default; @@ -196,6 +195,9 @@ class PoseGraph PoseGraph(const PoseGraph&) = default; PoseGraph& operator=(const PoseGraph&) = default; + // can be used for debugging + PoseGraph(const std::string& g2oFileName); + void addNode(const PoseGraphNode& node) { int id = node.getId(); @@ -228,8 +230,9 @@ class PoseGraph int getNumEdges() const { return int(edges.size()); } public: - Nodes nodes; - EdgeVector edges; + + std::map nodes; + std::vector edges; }; namespace Optimizer From f89247da574c2cfe106f7718e77c35ebed31ea84 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 11 Mar 2021 03:10:44 +0300 Subject: [PATCH 03/51] sparse solver fixes from DynaFu draft --- modules/rgbd/src/sparse_block_matrix.hpp | 53 +++++++++++++++--------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 0e607af639d..eb77ff390e5 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -24,7 +24,7 @@ namespace kinfu * \class BlockSparseMat * Naive implementation of Sparse Block Matrix */ -template +template struct BlockSparseMat { struct Point2iHash @@ -41,11 +41,11 @@ struct BlockSparseMat typedef Matx<_Tp, blockM, blockN> MatType; typedef std::unordered_map IDtoBlockValueMap; - BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} + BlockSparseMat(size_t _nBlocks) : nBlocks(_nBlocks), ijValue() {} - MatType& refBlock(int i, int j) + MatType& refBlock(size_t i, size_t j) { - Point2i p(i, j); + Point2i p((int)i, (int)j); auto it = ijValue.find(p); if (it == ijValue.end()) { @@ -58,18 +58,19 @@ struct BlockSparseMat { // Diagonal max length is the number of columns in the sparse matrix int diagLength = blockN * nBlocks; - cv::Mat diag = cv::Mat::zeros(diagLength, 1, CV_32F); + cv::Mat diag = cv::Mat::zeros(diagLength, 1, cv::DataType<_Tp>::type); for (int i = 0; i < diagLength; i++) { - diag.at(i, 0) = refElem(i, i); + diag.at<_Tp>(i, 0) = refElem(i, i); } return diag; } - float& refElem(int i, int j) + _Tp& refElem(size_t i, size_t j) { - Point2i ib(i / blockM, j / blockN), iv(i % blockM, j % blockN); + Point2i ib((int)(i / blockM), (int)(j / blockN)); + Point2i iv((int)(i % blockM), (int)(j % blockN)); return refBlock(ib.x, ib.y)(iv.x, iv.y); } @@ -78,7 +79,7 @@ struct BlockSparseMat { std::vector> tripletList; tripletList.reserve(ijValue.size() * blockM * blockN); - for (auto ijv : ijValue) + for (const auto& ijv : ijValue) { int xb = ijv.first.x, yb = ijv.first.y; MatType vblock = ijv.second; @@ -103,16 +104,28 @@ struct BlockSparseMat #endif size_t nonZeroBlocks() const { return ijValue.size(); } + BlockSparseMat<_Tp, blockM, blockN>& operator+=(const BlockSparseMat<_Tp, blockM, blockN>& other) + { + for (const auto& oijv : other.ijValue) + { + Point2i p = oijv.first; + MatType vblock = oijv.second; + this->refBlock(p.x, p.y) += vblock; + } + + return *this; + } + static constexpr float NON_ZERO_VAL_THRESHOLD = 0.0001f; - int nBlocks; + size_t nBlocks; IDtoBlockValueMap ijValue; }; //! Function to solve a sparse linear system of equations HX = B //! Requires Eigen -static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X, Mat& predB) +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, + OutputArray X, OutputArray predB = cv::noArray()) { - bool result = false; #if defined(HAVE_EIGEN) Eigen::SparseMatrix bigA = H.toEigen(); Eigen::VectorXf bigB; @@ -122,7 +135,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& if(!bigA.isApprox(bigAtranspose)) { CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); - return result; + return false; } Eigen::SimplicialLDLT> solver; @@ -131,29 +144,31 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-decompose" << std::endl; - result = false; + return false; } else { Eigen::VectorXf solutionX = solver.solve(bigB); - Eigen::VectorXf predBEigen = bigA * solutionX; if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-solve" << std::endl; - result = false; + return false; } else { eigen2cv(solutionX, X); - eigen2cv(predBEigen, predB); - result = true; + if (predB.needed()) + { + Eigen::VectorXf predBEigen = bigA * solutionX; + eigen2cv(predBEigen, predB); + } + return true; } } #else std::cout << "no eigen library" << std::endl; CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); #endif - return result; } } // namespace kinfu } // namespace cv From 92834f81973c1122d7975defdb94a6d164adc423 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 11 Mar 2021 03:15:29 +0300 Subject: [PATCH 04/51] Eigen cost function and parametrization removed + g2o reading fixed --- modules/rgbd/src/pose_graph.cpp | 70 +++++++-------------------- modules/rgbd/src/pose_graph.hpp | 85 ++++++--------------------------- 2 files changed, 31 insertions(+), 124 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 768c7b4548a..38d80a37ca4 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -99,7 +99,7 @@ PoseGraph::PoseGraph(const std::string& g2oFileName) : }; // for debugging purposes - int minId = 0, maxId = 1 << 24; + int minId = 0, maxId = 1 << 30; std::ifstream infile(g2oFileName.c_str()); if (!infile) @@ -143,9 +143,6 @@ PoseGraph::PoseGraph(const std::string& g2oFileName) : infile >> startId >> endId; Affine3d pose = readAffine(infile); - if (!((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId))) - continue; - Matx66d info; for (int i = 0; i < 6 && infile.good(); ++i) { @@ -158,8 +155,11 @@ PoseGraph::PoseGraph(const std::string& g2oFileName) : } } } - - edges.push_back(PoseGraphEdge(startId, endId, pose, info)); + + if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId)) + { + edges.push_back(PoseGraphEdge(startId, endId, pose, info)); + } } else { @@ -185,41 +185,21 @@ class MyQuaternionParameterization Quatd x(vx); Vec3d delta(delta_ptr); - const double norm_delta = norm(delta); - Quatd x_plus_delta; - if (norm_delta > 0.0) - { - const double sin_delta_by_delta = std::sin(norm_delta) / norm_delta; - - // Note, in the constructor w is first. - Quatd delta_q(std::cos(norm_delta), - sin_delta_by_delta * delta[0], - sin_delta_by_delta * delta[1], - sin_delta_by_delta * delta[2]); - x_plus_delta = delta_q * x; - } - else - { - x_plus_delta = x; - } + Quatd x_plus_delta = Quatd(0, delta[0], delta[1], delta[2]).exp() * x; - Vec4d xpd = x_plus_delta.toVec(); - x_plus_delta_ptr[0] = xpd[0]; - x_plus_delta_ptr[1] = xpd[1]; - x_plus_delta_ptr[2] = xpd[2]; - x_plus_delta_ptr[3] = xpd[3]; + *(Vec4d*)(x_plus_delta_ptr) = x_plus_delta.toVec(); return true; } bool ComputeJacobian(const double* x, double* jacobian) const override { - // clang-format off - jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; - jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; - jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; - jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; - // clang-format on + Vec4d vx(x); + Matx43d jm = Optimizer::expQuatJacobian(Quatd(vx)); + + for (int ii = 0; ii < 12; ii++) + jacobian[ii] = jm.val[ii]; + return true; } @@ -239,7 +219,7 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& ceres::LossFunction* lossFunction = nullptr; // TODO: Experiment with SE3 parameterization ceres::LocalParameterization* quatLocalParameterization = - new ceres::EigenQuaternionParameterization; + new MyQuaternionParameterization; for (const PoseGraphEdge& currEdge : poseGraph.edges) { @@ -248,28 +228,12 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; - // ------- - - Eigen::Matrix info; - cv2eigen(Matx66d(currEdge.information), info); - const Eigen::Matrix sqrt_information = info.llt().matrixL(); - Matx66d sqrtInfo; - eigen2cv(sqrt_information, sqrtInfo); - - ceres::CostFunction* costFunction = Pose3dErrorFunctor::create( - Pose3d(currEdge.transformation.rotation(), currEdge.transformation.translation()), - sqrtInfo); - - // ------- - - ceres::CostFunction* costFunction2 = Pose3dAnalyticCostFunction::create( + ceres::CostFunction* costFunction = Pose3dAnalyticCostFunction::create( Vec3d(currEdge.transformation.translation()), Quatd::createFromRotMat(Matx33d(currEdge.transformation.rotation())), currEdge.information); - // ------- - - problem.AddResidualBlock(costFunction2, lossFunction, + problem.AddResidualBlock(costFunction, lossFunction, sourcePose.t.val, sourcePose.vq.val, targetPose.t.val, targetPose.vq.val); problem.SetParameterization(sourcePose.vq.val, quatLocalParameterization); diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 963642ad776..3ab72a94f97 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -60,6 +60,11 @@ struct Pose3d return Affine3d(Quatd(vq).toRotMat3x3(QUAT_ASSUME_UNIT), t); } + Quatd getQuat() const + { + return Quatd(vq); + } + inline Pose3d inverse() const { Pose3d out; @@ -242,77 +247,6 @@ void optimize(PoseGraph& poseGraph); #if defined(CERES_FOUND) void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem); -//! Error Functor required for Ceres to obtain an auto differentiable cost function -class Pose3dErrorFunctor -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Pose3dErrorFunctor(const Pose3d& _poseMeasurement, const Matx66d& _sqrtInformation) - : poseMeasurement(_poseMeasurement) - { - cv2eigen(_sqrtInformation, sqrtInfo); - } - Pose3dErrorFunctor(const Pose3d& _poseMeasurement, - const Eigen::Matrix& _sqrtInformation) - : poseMeasurement(_poseMeasurement), sqrtInfo(_sqrtInformation) - { - } - - template - bool operator()(const T* const _pSourceTrans, const T* const _pSourceQuat, - const T* const _pTargetTrans, const T* const _pTargetQuat, T* _pResidual) const - { - Eigen::Map> sourceTrans(_pSourceTrans); - Eigen::Map> targetTrans(_pTargetTrans); - - //Eigen::Quaternion sourceQuat(_pSourceQuat[3], _pSourceQuat[0], _pSourceQuat[1], _pSourceQuat[2]); - //Eigen::Quaternion targetQuat(_pTargetQuat[3], _pTargetQuat[0], _pTargetQuat[1], _pTargetQuat[2]); - Eigen::Quaternion sourceQuat(_pSourceQuat[0], _pSourceQuat[1], _pSourceQuat[2], _pSourceQuat[3]); - Eigen::Quaternion targetQuat(_pTargetQuat[0], _pTargetQuat[1], _pTargetQuat[2], _pTargetQuat[3]); - - Eigen::Map> residual(_pResidual); - - //Eigen::Quaternion targetQuatInv = targetQuat.conjugate(); - - //Eigen::Quaternion relativeQuat = targetQuatInv * sourceQuat; - //Eigen::Matrix relativeTrans = targetQuatInv * (targetTrans - sourceTrans); - - Eigen::Quaternion sourceQuatInv = sourceQuat.conjugate(); - - Eigen::Quaternion relativeQuat = sourceQuatInv * targetQuat; - Eigen::Matrix relativeTrans = sourceQuatInv * (targetTrans - sourceTrans); - - //! Definition should actually be relativeQuat * poseMeasurement.r.conjugate() - Eigen::Quaternion pmr(T(poseMeasurement.vq[0]), T(poseMeasurement.vq[1]), T(poseMeasurement.vq[2]), T(poseMeasurement.vq[3])); - - //DEBUG - //Eigen::Quaternion deltaRot = pmr * relativeQuat.conjugate(); - Eigen::Quaternion deltaRot = relativeQuat.conjugate() * pmr; - - //DEBUG - //deltaRot = (deltaRot.w() < T(0)) ? deltaRot * Eigen::Quaternion(T(-1), T(0), T(0), T(0)) : deltaRot; - - Eigen::Matrix pmt(T(poseMeasurement.t[0]), T(poseMeasurement.t[1]), T(poseMeasurement.t[2])); - residual.template block<3, 1>(0, 0) = relativeTrans - pmt; - residual.template block<3, 1>(3, 0) = T(2.0) * deltaRot.vec(); - - residual.applyOnTheLeft(sqrtInfo.template cast()); - - return true; - } - - static ceres::CostFunction* create(const Pose3d& _poseMeasurement, - const Matx66f& _sqrtInformation) - { - return new ceres::AutoDiffCostFunction( - new Pose3dErrorFunctor(_poseMeasurement, _sqrtInformation)); - } - - private: - const Pose3d poseMeasurement; - Eigen::Matrix sqrtInfo; -}; - // matrix form of Im(a) const Matx44d M_Im{ 0, 0, 0, 0, 0, 1, 0, 0, @@ -353,6 +287,15 @@ inline Matx44d m_right(Quatd q) z, y, -x, w }; } +inline Matx43d expQuatJacobian(Quatd q) +{ + double w = q.w, x = q.x, y = q.y, z = q.z; + return Matx43d(-x, -y, -z, + w, z, -y, + -z, w, x, + y, -x, w); +} + // concatenate matrices vertically template static inline Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) From fdd92ce330b3b93ec662a608ec962867acf84bb4 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Fri, 12 Mar 2021 02:45:42 +0300 Subject: [PATCH 05/51] refactored: pose error, pose graph edge, pose graph node --- modules/rgbd/src/pose_graph.cpp | 9 +- modules/rgbd/src/pose_graph.hpp | 233 ++++++++++++++++---------------- 2 files changed, 120 insertions(+), 122 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 38d80a37ca4..a6621117930 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -228,14 +228,11 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; - ceres::CostFunction* costFunction = Pose3dAnalyticCostFunction::create( - Vec3d(currEdge.transformation.translation()), - Quatd::createFromRotMat(Matx33d(currEdge.transformation.rotation())), - currEdge.information); + ceres::CostFunction* costFunction = Pose3dAnalyticCostFunction::create(Vec3d(currEdge.pose.t), currEdge.pose.getQuat(), currEdge.information); problem.AddResidualBlock(costFunction, lossFunction, - sourcePose.t.val, sourcePose.vq.val, - targetPose.t.val, targetPose.vq.val); + sourcePose.t.val, sourcePose.vq.val, + targetPose.t.val, targetPose.vq.val); problem.SetParameterization(sourcePose.vq.val, quatLocalParameterization); problem.SetParameterization(targetPose.vq.val, quatLocalParameterization); } diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 3ab72a94f97..d873290b750 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -29,6 +29,27 @@ namespace kinfu * Detailed description */ +// Cholesky decomposition of symmetrical 6x6 matrix +inline cv::Matx66d llt6(Matx66d m) +{ + Matx66d L; + for (int i = 0; i < 6; i++) + { + for (int j = 0; j < (i + 1); j++) + { + double sum = 0; + for (int k = 0; k < j; k++) + sum += L(i, k) * L(j, k); + + if (i == j) + L(i, i) = sqrt(m(i, i) - sum); + else + L(i, j) = (1.0 / L(j, j) * (m(i, j) - sum)); + } + } + return L; +} + struct Pose3d { Vec3d t; @@ -83,7 +104,7 @@ struct PoseGraphNode { public: explicit PoseGraphNode(int _nodeId, const Affine3d& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose) + : nodeId(_nodeId), isFixed(false) { se3Pose = Pose3d(_pose.rotation(), _pose.translation()); } @@ -92,18 +113,15 @@ struct PoseGraphNode int getId() const { return nodeId; } inline Affine3d getPose() const { - pose = se3Pose.getAffine(); - return pose; + return se3Pose.getAffine(); } void setPose(const Affine3d& _pose) { - pose = _pose; - se3Pose = Pose3d(pose.rotation(), pose.translation()); + se3Pose = Pose3d(_pose.rotation(), _pose.translation()); } void setPose(const Pose3d& _pose) { se3Pose = _pose; - pose = se3Pose.getAffine(); } void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } @@ -111,7 +129,6 @@ struct PoseGraphNode public: int nodeId; bool isFixed; - mutable Affine3d pose; Pose3d se3Pose; }; @@ -127,8 +144,9 @@ struct PoseGraphEdge const Matx66f& _information = Matx66f::eye()) : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), - transformation(_transformation), - information(_information) + pose(_transformation.rotation(), _transformation.translation()), + information(_information), + sqrtInfo(llt6(_information)) { } virtual ~PoseGraphEdge() = default; @@ -147,8 +165,9 @@ struct PoseGraphEdge public: int sourceNodeId; int targetNodeId; - Affine3f transformation; + Pose3d pose; Matx66f information; + Matx66f sqrtInfo; }; //! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold @@ -296,6 +315,7 @@ inline Matx43d expQuatJacobian(Quatd q) y, -x, w); } + // concatenate matrices vertically template static inline Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) @@ -342,6 +362,78 @@ Matx<_Tp, m, n + k> concatHor(const Matx<_Tp, m, n>& a, const Matx<_Tp, m, k>& b } +inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd targetQuat, Vec3d targetTrans, + Quatd rotMeasured, Vec3d transMeasured, Matx66d sqrtInfoMatrix, bool needJacobians, + Matx& sqj, Matx& stj, + Matx& tqj, Matx& ttj, + Vec6d& res) +{ + // err_r = 2*Im(conj(rel_r) * measure_r) = 2*Im(conj(target_r) * source_r * measure_r) + // err_t = conj(source_r) * (target_t - source_t) * source_r - measure_t + + Quatd sourceQuatInv = sourceQuat.conjugate(); + Vec3d deltaTrans = targetTrans - sourceTrans; + + Quatd relativeQuat = sourceQuatInv * targetQuat; + Vec3d relativeTrans = sourceQuatInv.toRotMat3x3(cv::QUAT_ASSUME_UNIT) * deltaTrans; + + //! Definition should actually be relativeQuat * rotMeasured.conjugate() + Quatd deltaRot = relativeQuat.conjugate() * rotMeasured; + + Vec3d terr = relativeTrans - transMeasured; + Vec3d rerr = 2.0 * Vec3d(deltaRot.x, deltaRot.y, deltaRot.z); + Vec6d rterr(terr[0], terr[1], terr[2], rerr[0], rerr[1], rerr[2]); + + res = sqrtInfoMatrix * rterr; + + if (needJacobians) + { + // d(err_r) = 2*Im(d(conj(target_r) * source_r * measure_r)) = < measure_r is constant > = + // 2*Im((conj(d(target_r)) * source_r + conj(target_r) * d(source_r)) * measure_r) + // d(target_r) == 0: + // # d(err_r) = 2*Im(conj(target_r) * d(source_r) * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) * V(d(source_r)) + // # d(err_r) / d(source_r) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) + Matx34d drdsq = 2.0 * (m_right(rotMeasured) * m_left(targetQuat.conjugate())).get_minor<3, 4>(1, 0); + + // d(source_r) == 0: + // # d(err_r) = 2*Im(conj(d(target_r)) * source_r * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj * V(d(target_r)) + // # d(err_r) / d(target_r) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj + Matx34d drdtq = 2.0 * (m_right(sourceQuat * rotMeasured) * M_Conj).get_minor<3, 4>(1, 0); + + // d(err_t) = d(conj(source_r) * (target_t - source_t) * source_r) = + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // conj(d(source_r)) * (target_t - source_t) * source_r + + // conj(source_r) * (target_t - source_t) * d(source_r) = + // + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // d(*_t) == 0: + // # d(err_t) = 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // # V(d(err_t)) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) * V(d(source_r)) + // # d(err_t) / d(source_r) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) + Matx34d dtdsq = 2 * m_left(sourceQuatInv * Quatd(0, deltaTrans[0], deltaTrans[1], deltaTrans[2])).get_minor<3, 4>(1, 0); + // deltaTrans is rotated by sourceQuatInv, so the jacobian is rot matrix of sourceQuatInv by +1 or -1 + Matx33d dtdtt = sourceQuatInv.toRotMat3x3(QUAT_ASSUME_UNIT); + Matx33d dtdst = -dtdtt; + + Matx33d z; + sqj = concatVert(dtdsq, drdsq); + tqj = concatVert(Matx34d(), drdtq); + stj = concatVert(dtdst, z); + ttj = concatVert(dtdtt, z); + + stj = sqrtInfoMatrix * stj; + ttj = sqrtInfoMatrix * ttj; + sqj = sqrtInfoMatrix * sqj; + tqj = sqrtInfoMatrix * tqj; + } + + return res.ddot(res); +} + + class Pose3dAnalyticCostFunction : public ceres::SizedCostFunction<6, 3, 4, 3, 4> { public: @@ -353,26 +445,7 @@ class Pose3dAnalyticCostFunction : public ceres::SizedCostFunction<6, 3, 4, 3, 4 sqrtInfoMatrix(llt6(infoMatrix)) { } - static Matx66d llt6(Matx66d m) - { - Matx66d L; - for (int i = 0; i < 6; i++) - { - for (int j = 0; j < (i + 1); j++) - { - double sum = 0; - for (int k = 0; k < j; k++) - sum += L(i, k) * L(j, k); - - if (i == j) - L(i, i) = sqrt(m(i, i) - sum); - else - L(i, j) = (1.0 / L(j, j) * (m(i, j) - sum)); - } - } - return L; - } - + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override @@ -384,99 +457,27 @@ class Pose3dAnalyticCostFunction : public ceres::SizedCostFunction<6, 3, 4, 3, 4 Vec4d p3(parameters[3]); Quatd targetQuat(p3); - // err_r = 2*Im(conj(rel_r) * measure_r) = 2*Im(conj(target_r) * source_r * measure_r) - // err_t = conj(source_r) * (target_t - source_t) * source_r - measure_t - Vec6d res; - - Quatd sourceQuatInv = sourceQuat.conjugate(); - Vec3d deltaTrans = targetTrans - sourceTrans; - - Quatd relativeQuat = sourceQuatInv * targetQuat; - Vec3d relativeTrans = sourceQuatInv.toRotMat3x3(cv::QUAT_ASSUME_UNIT) * deltaTrans; - - //! Definition should actually be relativeQuat * rotMeasured.conjugate() - Quatd deltaRot = relativeQuat.conjugate() * rotMeasured; - - Vec3d terr = relativeTrans - transMeasured; - Vec3d rerr = 2.0 * Vec3d(deltaRot.x, deltaRot.y, deltaRot.z); - Vec6d rterr(terr[0], terr[1], terr[2], rerr[0], rerr[1], rerr[2]); - - res = sqrtInfoMatrix * rterr; + Matx stj, ttj; + Matx sqj, tqj; + poseError(sourceQuat, sourceTrans, targetQuat, targetTrans, + rotMeasured, transMeasured, sqrtInfoMatrix, /* needJacobians = */ (bool)(jacobians), + sqj, stj, tqj, ttj, res); if (jacobians) { - Matx stj, ttj; - Matx sqj, tqj; - - // d(err_r) = 2*Im(d(conj(target_r) * source_r * measure_r)) = < measure_r is constant > = - // 2*Im((conj(d(target_r)) * source_r + conj(target_r) * d(source_r)) * measure_r) - // d(target_r) == 0: - // # d(err_r) = 2*Im(conj(target_r) * d(source_r) * measure_r) - // # V(d(err_r)) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) * V(d(source_r)) - // # d(err_r) / d(source_r) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) - Matx34d drdsq = 2.0 * (m_right(rotMeasured) * m_left(targetQuat.conjugate())).get_minor<3, 4>(1, 0); - - // d(source_r) == 0: - // # d(err_r) = 2*Im(conj(d(target_r)) * source_r * measure_r) - // # V(d(err_r)) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj * V(d(target_r)) - // # d(err_r) / d(target_r) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj - Matx34d drdtq = 2.0 * (m_right(sourceQuat * rotMeasured) * M_Conj).get_minor<3, 4>(1, 0); - - // d(err_t) = d(conj(source_r) * (target_t - source_t) * source_r) = - // conj(source_r) * (d(target_t) - d(source_t)) * source_r + - // conj(d(source_r)) * (target_t - source_t) * source_r + - // conj(source_r) * (target_t - source_t) * d(source_r) = - // - // conj(source_r) * (d(target_t) - d(source_t)) * source_r + - // 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) - // d(*_t) == 0: - // # d(err_t) = 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) - // # V(d(err_t)) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) * V(d(source_r)) - // # d(err_t) / d(source_r) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) - Vec3d td = targetTrans - sourceTrans; - Matx34d dtdsq = 2 * m_left(sourceQuatInv * Quatd(0, deltaTrans[0], deltaTrans[1], deltaTrans[2])).get_minor<3, 4>(1, 0); - // deltaTrans is rotated by sourceQuatInv, so the jacobian is rot matrix of sourceQuatInv by +1 or -1 - Matx33d dtdtt = sourceQuatInv.toRotMat3x3(QUAT_ASSUME_UNIT); - Matx33d dtdst = - dtdtt; - - Matx33d z; - sqj = concatVert(dtdsq, drdsq); - tqj = concatVert(Matx34d(), drdtq); - stj = concatVert(dtdst, z); - ttj = concatVert(dtdtt, z); - - stj = sqrtInfoMatrix * stj; - ttj = sqrtInfoMatrix * ttj; - sqj = sqrtInfoMatrix * sqj; - tqj = sqrtInfoMatrix * tqj; - - // sourceTrans - if (jacobians[0]) - { - for (int i = 0; i < 6 * 3; i++) - jacobians[0][i] = stj.val[i]; - } - - // sourceQuat - if (jacobians[1]) - { - for (int i = 0; i < 6 * 4; i++) - jacobians[1][i] = sqj.val[i]; - } + // source trans, source quat, target trans, target quat + double* ptrs[4] = { stj.val, sqj.val, ttj.val, tqj.val }; + size_t sizes[4] = { 6 * 3, 6 * 4, 6 * 3, 6 * 4 }; - // targetTrans - if (jacobians[2]) + for (int k = 0; k < 4; k++) { - for (int i = 0; i < 6 * 3; i++) - jacobians[2][i] = ttj.val[i]; - } + if (jacobians[k]) + { + for (int i = 0; i < sizes[k]; i++) + jacobians[k][i] = ptrs[k][i]; - // targetQuat - if (jacobians[3]) - { - for (int i = 0; i < 6 * 4; i++) - jacobians[3][i] = tqj.val[i]; + } } } From f71f862fea190c92a9eccf29f68a4540f0a0a503 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Fri, 12 Mar 2021 02:46:27 +0300 Subject: [PATCH 06/51] sparse solve: templated --- modules/rgbd/src/sparse_block_matrix.hpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index eb77ff390e5..908b4f83000 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -123,22 +123,23 @@ struct BlockSparseMat //! Function to solve a sparse linear system of equations HX = B //! Requires Eigen -static bool sparseSolve(const BlockSparseMat& H, const Mat& B, +template +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, OutputArray X, OutputArray predB = cv::noArray()) { #if defined(HAVE_EIGEN) - Eigen::SparseMatrix bigA = H.toEigen(); - Eigen::VectorXf bigB; + Eigen::SparseMatrix bigA = H.toEigen(); + Eigen::Matrix bigB; cv2eigen(B, bigB); - Eigen::SparseMatrix bigAtranspose = bigA.transpose(); + Eigen::SparseMatrix bigAtranspose = bigA.transpose(); if(!bigA.isApprox(bigAtranspose)) { CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); return false; } - Eigen::SimplicialLDLT> solver; + Eigen::SimplicialLDLT> solver; solver.compute(bigA); if (solver.info() != Eigen::Success) @@ -148,7 +149,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, } else { - Eigen::VectorXf solutionX = solver.solve(bigB); + Eigen::Matrix solutionX = solver.solve(bigB); if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-solve" << std::endl; @@ -159,7 +160,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, eigen2cv(solutionX, X); if (predB.needed()) { - Eigen::VectorXf predBEigen = bigA * solutionX; + Eigen::Matrix predBEigen = bigA * solutionX; eigen2cv(predBEigen, predB); } return true; From 01b1608dfe122d12193d06bbeea2e549159f0978 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Fri, 12 Mar 2021 02:47:14 +0300 Subject: [PATCH 07/51] MyOptimize(): 1st version --- modules/rgbd/src/pose_graph.cpp | 218 ++++++++++++++++++++++++++++++++ modules/rgbd/src/pose_graph.hpp | 3 + 2 files changed, 221 insertions(+) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index a6621117930..6b69ffb751d 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -249,6 +249,224 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& } #endif +void Optimizer::MyOptimize(PoseGraph& poseGraph) +{ + PoseGraph poseGraphOriginal = poseGraph; + + if (!poseGraphOriginal.isValid()) + { + CV_Error(Error::StsBadArg, + "Invalid PoseGraph that is either not connected or has invalid nodes"); + return; + } + + int numNodes = poseGraph.getNumNodes(); + int numEdges = poseGraph.getNumEdges(); + + // Allocate indices for nodes + std::vector placesIds; + std::map idToPlace; + for (const auto& ni : poseGraph.nodes) + { + if (!ni.second.isPoseFixed()) + { + idToPlace[ni.first] = placesIds.size(); + placesIds.push_back(ni.first); + } + } + + int nVarNodes = placesIds.size(); + if (!nVarNodes) + { + CV_Error(Error::StsBadArg, "PoseGraph has no non-constant nodes, no optimization to be done"); + return; + } + + if (numEdges == 0) + { + CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); + return; + } + + std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; + + // estimate current energy + + auto calcEnergy = [&poseGraph](const std::map& nodes) -> double + { + double totalErr = 0; + for (const auto& e : poseGraph.edges) + { + Pose3d srcP = nodes.at(e.getSourceNodeId()).se3Pose; + Pose3d tgtP = nodes.at(e.getTargetNodeId()).se3Pose; + + Vec6d res; + Matx stj, ttj; + Matx sqj, tqj; + double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, + e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ false, + sqj, stj, tqj, ttj, res); + + totalErr += err; + } + return totalErr; + }; + double energy = calcEnergy(poseGraph.nodes); + double startEnergy = energy; + double oldEnergy = energy; + + std::cout << "#s" << " energy: " << energy << std::endl; + + const double initialLambdaLevMarq = 0.1; + const double lmUpFactor = 2.0; + const double lmDownFactor = 3.0; + const double coeffILM = 0.1f; + const double minTolerance = 1e-6; + unsigned int maxIterations = 100; + + double lambdaLevMarq = initialLambdaLevMarq; + unsigned int iter = 0; + bool done = false; + while (!done) + { + BlockSparseMat jtj(nVarNodes); + std::vector jtb(nVarNodes * 6); + + // caching nodes jacobians + std::vector> cachedJac; + for (auto id : placesIds) + { + Pose3d p = poseGraph.nodes.at(id).se3Pose; + Matx43d qj = expQuatJacobian(p.getQuat()); + // x node layout is (rot_x, rot_y, rot_z, trans_x, trans_y, trans_z) + // pose layout is (q_w, q_x, q_y, q_z, trans_x, trans_y, trans_z) + Matx j = concatVert(concatHor(qj, Matx43d()), + concatHor(Matx33d(), Matx33d::eye())); + cachedJac.push_back(j); + } + + // fill jtj and jtb + for (const auto& e : poseGraph.edges) + { + int srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); + Pose3d srcP = poseGraph.nodes.at(srcId).se3Pose; + Pose3d tgtP = poseGraph.nodes.at(dstId).se3Pose; + + Vec6d res; + Matx stj, ttj; + Matx sqj, tqj; + double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, + e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ true, + sqj, stj, tqj, ttj, res); + + size_t srcPlace = idToPlace[srcId], dstPlace = idToPlace[dstId]; + Matx66d sj = concatHor(sqj, stj) * cachedJac[srcPlace]; + Matx66d tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; + + jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; + jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; + Matx66d sjttj = sj.t() * tj; + jtj.refBlock(srcPlace, dstPlace) += sjttj; + jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); + + Vec6f jtbSrc = sj.t() * res, jtbDst = tj.t() * res; + for (int i = 0; i < 6; i++) + { + jtb[6 * srcPlace + i] += jtbSrc[i]; + jtb[6 * dstPlace + i] += jtbDst[i]; + } + } + + std::cout << "#LM#s" << " energy: " << energy << std::endl; + + // Solve using LevMarq and get delta transform + bool enough = false; + + // Save original diagonal of jtj matrix + std::vector diag(nVarNodes*6); + for (int i = 0; i < nVarNodes*6; i++) + { + diag[i] = jtj.refElem(i, i); + } + + decltype(poseGraph.nodes) tempNodes = poseGraph.nodes; + + while (!enough && !done) + { + // form LevMarq matrix + for (int i = 0; i < nVarNodes*6; i++) + { + float v = diag[i]; + jtj.refElem(i, i) = v + lambdaLevMarq * (v + coeffILM); + } + + // use double or convert everything to float + std::vector x; + bool solved = kinfu::sparseSolve(jtj, Mat(jtb), x); + + if (solved) + { + tempNodes = poseGraph.nodes; + + // Update temp nodes using x + for (int i = 0; i < nVarNodes; i++) + { + Vec6d dx(&x[i * 6]); + Vec3d deltaRot(dx[0], dx[1], dx[2]), deltaTrans(dx[3], dx[4], dx[5]); + Pose3d& p = tempNodes.at(placesIds[i]).se3Pose; + + p.vq = (Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.getQuat()).toVec(); + p.t += deltaTrans; + } + + // calc energy with temp nodes + energy = calcEnergy(tempNodes); + + std::cout << "#LM#" << iter; + std::cout << " energy: " << energy; + } + else + { + std::cout << "not solved" << std::endl; + } + + std::cout << std::endl; + + if (!solved || (energy > oldEnergy)) + { + // failed to optimize, increase lambda and repeat + + lambdaLevMarq *= lmUpFactor; + + std::cout << "LM up, old energy = " << oldEnergy << std::endl; + } + else + { + // optimized successfully, decrease lambda and set variables for next iteration + enough = true; + + lambdaLevMarq /= lmDownFactor; + + poseGraph.nodes = tempNodes; + + std::cout << "#" << iter; + std::cout << " energy: " << energy; + std::cout << std::endl; + + oldEnergy = energy; + + std::cout << "LM down" << std::endl; + } + + iter++; + + done = (iter >= maxIterations) || (startEnergy - energy) < minTolerance * startEnergy; + } + } + + // TODO: set timers & produce report +} + void Optimizer::optimize(PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index d873290b750..8c84c9aa325 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -261,6 +261,9 @@ class PoseGraph namespace Optimizer { + +void MyOptimize(PoseGraph& poseGraph); + void optimize(PoseGraph& poseGraph); #if defined(CERES_FOUND) From ff782d1a81238a6a81e1558893b5b35669b6dad0 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Fri, 12 Mar 2021 03:53:56 +0300 Subject: [PATCH 08/51] several fixes and TODOs for future --- modules/rgbd/src/pose_graph.cpp | 65 ++++++++++++++++++++++++--------- 1 file changed, 47 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 6b69ffb751d..46f6ec6e5f4 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -251,6 +251,7 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& void Optimizer::MyOptimize(PoseGraph& poseGraph) { + //TODO: no copying PoseGraph poseGraphOriginal = poseGraph; if (!poseGraphOriginal.isValid()) @@ -309,7 +310,7 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) totalErr += err; } - return totalErr; + return totalErr*0.5; }; double energy = calcEnergy(poseGraph.nodes); double startEnergy = energy; @@ -349,8 +350,13 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) for (const auto& e : poseGraph.edges) { int srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); - Pose3d srcP = poseGraph.nodes.at(srcId).se3Pose; - Pose3d tgtP = poseGraph.nodes.at(dstId).se3Pose; + const PoseGraphNode& srcNode = poseGraph.nodes.at(srcId); + const PoseGraphNode& dstNode = poseGraph.nodes.at(dstId); + + Pose3d srcP = srcNode.se3Pose; + Pose3d tgtP = dstNode.se3Pose; + bool srcFixed = srcNode.isPoseFixed(); + bool dstFixed = dstNode.isPoseFixed(); Vec6d res; Matx stj, ttj; @@ -359,21 +365,43 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ true, sqj, stj, tqj, ttj, res); - size_t srcPlace = idToPlace[srcId], dstPlace = idToPlace[dstId]; - Matx66d sj = concatHor(sqj, stj) * cachedJac[srcPlace]; - Matx66d tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; + size_t srcPlace, dstPlace; + Matx66d sj, tj; + if (!srcFixed) + { + srcPlace = idToPlace.at(srcId); + sj = concatHor(sqj, stj) * cachedJac[srcPlace]; + + jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; - jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; - jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; - Matx66d sjttj = sj.t() * tj; - jtj.refBlock(srcPlace, dstPlace) += sjttj; - jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); + Vec6f jtbSrc = sj.t() * res; + for (int i = 0; i < 6; i++) + { + //TODO: there were no minus, check if this is correct + jtb[6 * srcPlace + i] += - jtbSrc[i]; + } + } - Vec6f jtbSrc = sj.t() * res, jtbDst = tj.t() * res; - for (int i = 0; i < 6; i++) + if (!dstFixed) + { + dstPlace = idToPlace.at(dstId); + tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; + + jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; + + Vec6f jtbDst = tj.t() * res; + for (int i = 0; i < 6; i++) + { + //TODO: there were no minus, check if this is correct + jtb[6 * dstPlace + i] += -jtbDst[i]; + } + } + + if (!(srcFixed || dstFixed)) { - jtb[6 * srcPlace + i] += jtbSrc[i]; - jtb[6 * dstPlace + i] += jtbDst[i]; + Matx66d sjttj = sj.t() * tj; + jtj.refBlock(srcPlace, dstPlace) += sjttj; + jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); } } @@ -438,7 +466,7 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) lambdaLevMarq *= lmUpFactor; - std::cout << "LM up, old energy = " << oldEnergy << std::endl; + std::cout << "LM up: " << lambdaLevMarq << ", old energy = " << oldEnergy << std::endl; } else { @@ -455,12 +483,13 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) oldEnergy = energy; - std::cout << "LM down" << std::endl; + std::cout << "LM down: " << lambdaLevMarq << std::endl; } iter++; - done = (iter >= maxIterations) || (startEnergy - energy) < minTolerance * startEnergy; + //TODO: fix this + done = (iter >= maxIterations); // || (startEnergy - energy) < minTolerance * startEnergy; } } From 9b2fa9d3702f0e74eeea3abd7fed10a2ca4facc3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sat, 20 Mar 2021 00:24:26 +0300 Subject: [PATCH 09/51] sparse block matrix: val functions, template type --- modules/rgbd/src/sparse_block_matrix.hpp | 50 +++++++++++++++++------- 1 file changed, 36 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 908b4f83000..790bc725cf0 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -43,18 +43,47 @@ struct BlockSparseMat BlockSparseMat(size_t _nBlocks) : nBlocks(_nBlocks), ijValue() {} + void clear() + { + ijValue.clear(); + } + MatType& refBlock(size_t i, size_t j) { Point2i p((int)i, (int)j); auto it = ijValue.find(p); if (it == ijValue.end()) { - it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>::zeros() }).first; + it = ijValue.insert({ p, MatType::zeros() }).first; } return it->second; } - Mat diagonal() + _Tp& refElem(size_t i, size_t j) + { + Point2i ib((int)(i / blockM), (int)(j / blockN)); + Point2i iv((int)(i % blockM), (int)(j % blockN)); + return refBlock(ib.x, ib.y)(iv.x, iv.y); + } + + MatType valBlock(size_t i, size_t j) const + { + Point2i p((int)i, (int)j); + auto it = ijValue.find(p); + if (it == ijValue.end()) + return MatType::zeros(); + else + return it->second; + } + + _Tp valElem(size_t i, size_t j) const + { + Point2i ib((int)(i / blockM), (int)(j / blockN)); + Point2i iv((int)(i % blockM), (int)(j % blockN)); + return valBlock(ib.x, ib.y)(iv.x, iv.y); + } + + Mat diagonal() const { // Diagonal max length is the number of columns in the sparse matrix int diagLength = blockN * nBlocks; @@ -62,22 +91,15 @@ struct BlockSparseMat for (int i = 0; i < diagLength; i++) { - diag.at<_Tp>(i, 0) = refElem(i, i); + diag.at<_Tp>(i, 0) = valElem(i, i); } return diag; } - _Tp& refElem(size_t i, size_t j) - { - Point2i ib((int)(i / blockM), (int)(j / blockN)); - Point2i iv((int)(i % blockM), (int)(j % blockN)); - return refBlock(ib.x, ib.y)(iv.x, iv.y); - } - #if defined(HAVE_EIGEN) Eigen::SparseMatrix<_Tp> toEigen() const { - std::vector> tripletList; + std::vector> tripletList; tripletList.reserve(ijValue.size() * blockM * blockN); for (const auto& ijv : ijValue) { @@ -87,10 +109,10 @@ struct BlockSparseMat { for (int j = 0; j < blockN; j++) { - float val = vblock(i, j); + _Tp val = vblock(i, j); if (abs(val) >= NON_ZERO_VAL_THRESHOLD) { - tripletList.push_back(Eigen::Triplet(blockM * xb + i, blockN * yb + j, val)); + tripletList.push_back(Eigen::Triplet<_Tp>(blockM * xb + i, blockN * yb + j, val)); } } } @@ -116,7 +138,7 @@ struct BlockSparseMat return *this; } - static constexpr float NON_ZERO_VAL_THRESHOLD = 0.0001f; + static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001); size_t nBlocks; IDtoBlockValueMap ijValue; }; From ccb4262279b5adef4404a161c96eba7965f7c475 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sat, 20 Mar 2021 00:27:18 +0300 Subject: [PATCH 10/51] works at Ceres quality (cleanup needed) --- modules/rgbd/src/pose_graph.cpp | 236 ++++++++++++++++++++++++++++---- 1 file changed, 209 insertions(+), 27 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 46f6ec6e5f4..8669b1974d2 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -249,6 +249,24 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& } #endif +//DEBUG +void writePg(const PoseGraph& pg, std::string fname) +{ + std::fstream of(fname, std::fstream::out); + for (const auto& n : pg.nodes) + { + Point3d d = n.second.getPose().translation(); + of << "v " << d.x << " " << d.y << " " << d.z << std::endl; + } + for (const auto& e : pg.edges) + { + int sid = e.sourceNodeId, tid = e.targetNodeId; + of << "l " << sid + 1 << " " << tid + 1 << std::endl; + } + of.close(); +}; + + void Optimizer::MyOptimize(PoseGraph& poseGraph) { //TODO: no copying @@ -291,8 +309,11 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; - // estimate current energy + size_t nVars = nVarNodes * 6; + BlockSparseMat jtj(nVarNodes); + std::vector jtb(nVars); + // estimate current energy auto calcEnergy = [&poseGraph](const std::map& nodes) -> double { double totalErr = 0; @@ -312,26 +333,70 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) } return totalErr*0.5; }; + + // from Ceres, equation energy change: + // eq. energy = 1/2 * (residuals + J * step)^2 = + // 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) + // eq. energy change = 1/2 * residuals^2 - eq. energy = + // residuals^T * J * step + 1/2 * (J*step)^T * J * step = + // (residuals^T * J + 1/2 * step^T * J^T * J) * step = + // step^T * ((residuals^T * J)^T + 1/2 * (step^T * J^T * J)^T) = + // 1/2 * step^T * (2 * J^T * residuals + J^T * J * step) = + // 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag - LMDiag) * step) = + // 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag) * step - LMDiag * step) = + // 1/2 * step^T * (J^T * residuals - LMDiag * step) = + // 1/2 * x^T * (jtb - lmDiag^T * x) + auto calcJacCostChange = [nVars, &jtb](const std::vector& x, const std::vector& lmDiag) -> double + { + double jdiag = 0.0; + for (int i = 0; i < nVars; i++) + { + jdiag += x[i] * (jtb[i] - lmDiag[i] * x[i]); + } + double costChange = jdiag * 0.5; + return costChange; + }; + double energy = calcEnergy(poseGraph.nodes); double startEnergy = energy; double oldEnergy = energy; std::cout << "#s" << " energy: " << energy << std::endl; - const double initialLambdaLevMarq = 0.1; - const double lmUpFactor = 2.0; - const double lmDownFactor = 3.0; - const double coeffILM = 0.1f; - const double minTolerance = 1e-6; - unsigned int maxIterations = 100; - + // options + // stop conditions + const unsigned int maxIterations = 100; + const double maxGradientTolerance = 1e-6; + const double stepNorm2Tolerance = 1e-6; + const double relEnergyDeltaTolerance = 1e-6; + // normalize jac columns for better conditioning + const bool jacobiScaling = true; + const double minDiag = 1e-6; + const double maxDiag = 1e32; + + const double initialLambdaLevMarq = 0.0001; + const double initialLmUpFactor = 2.0; + const double initialLmDownFactor = 3.0; + + // finish reasons + bool tooLong = false; // => not found + bool smallGradient = false; // => found + bool smallStep = false; // => found + bool smallEnergyDelta = false; // => found + + // column scale inverted, for jacobi scaling + std::vector di(nVars); + + double lmUpFactor = initialLmUpFactor; + double decreaseFactorLevMarq = 2.0; double lambdaLevMarq = initialLambdaLevMarq; + unsigned int iter = 0; bool done = false; while (!done) { - BlockSparseMat jtj(nVarNodes); - std::vector jtb(nVarNodes * 6); + jtj.clear(); + std::fill(jtb.begin(), jtb.end(), 0.0); // caching nodes jacobians std::vector> cachedJac; @@ -377,7 +442,6 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) Vec6f jtbSrc = sj.t() * res; for (int i = 0; i < 6; i++) { - //TODO: there were no minus, check if this is correct jtb[6 * srcPlace + i] += - jtbSrc[i]; } } @@ -392,7 +456,6 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) Vec6f jtbDst = tj.t() * res; for (int i = 0; i < 6; i++) { - //TODO: there were no minus, check if this is correct jtb[6 * dstPlace + i] += -jtbDst[i]; } } @@ -406,34 +469,119 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) } std::cout << "#LM#s" << " energy: " << energy << std::endl; - - // Solve using LevMarq and get delta transform - bool enough = false; - // Save original diagonal of jtj matrix - std::vector diag(nVarNodes*6); - for (int i = 0; i < nVarNodes*6; i++) + // do the jacobian conditioning improvement used in Ceres + if (jacobiScaling) + { + // L2-normalize each jacobian column + // vec d = {d_j = sum(J_ij^2) for each column j of J} = get_diag{ J^T * J } + // di = { 1/(1+sqrt(d_j)) }, extra +1 to avoid div by zero + if (iter == 0) + { + for (int i = 0; i < nVars; i++) + { + double ds = sqrt(jtj.valElem(i, i)) + 1.0; + di[i] = 1.0 / ds; + } + } + + // J := J * d_inv, d_inv = make_diag(di) + // J^T*J := (J * d_inv)^T * J * d_inv = diag(di)* (J^T * J)* diag(di) = eltwise_mul(J^T*J, di*di^T) + // J^T*b := (J * d_inv)^T * b = d_inv^T * J^T*b = eltwise_mul(J^T*b, di) + + // scaling J^T*J + for (auto& ijv : jtj.ijValue) + { + Point2i bpt = ijv.first; + Matx66d& m = ijv.second; + for (int i = 0; i < 6; i++) + { + for (int j = 0; j < 6; j++) + { + Point2i pt(bpt.x * 6 + i, bpt.y * 6 + j); + m(i, j) *= di[pt.x] * di[pt.y]; + } + } + } + + // scaling J^T*b + for (int i = 0; i < nVars; i++) + { + jtb[i] *= di[i]; + } + } + + double gradientMax = 0.0; + // gradient max + for (int i = 0; i < nVars; i++) + { + gradientMax = std::max(gradientMax, abs(jtb[i])); + } + + // Save original diagonal of jtj matrix for LevMarq + std::vector diag(nVars); + for (int i = 0; i < nVars; i++) { - diag[i] = jtj.refElem(i, i); + diag[i] = jtj.valElem(i, i); } + + // Solve using LevMarq and get delta transform + bool enough = false; decltype(poseGraph.nodes) tempNodes = poseGraph.nodes; while (!enough && !done) { // form LevMarq matrix - for (int i = 0; i < nVarNodes*6; i++) + std::vector lmDiag(nVars); + for (int i = 0; i < nVars; i++) { - float v = diag[i]; - jtj.refElem(i, i) = v + lambdaLevMarq * (v + coeffILM); + double v = diag[i]; + + //double ld = lambdaLevMarq * (v + coeffILM); + double ld = std::min(max(v * lambdaLevMarq, minDiag), maxDiag); + + lmDiag[i] = ld; + + jtj.refElem(i, i) = v + ld; + + //DEBUG + //std::cout << jtj.refElem(i, i) << " "; } + std::cout << std::endl; + + std::cout << "sparse solve..."; + // use double or convert everything to float std::vector x; bool solved = kinfu::sparseSolve(jtj, Mat(jtb), x); + std::cout << "solve finished: " << std::endl; + + double costChange = 0.0; + double jacCostChange = 0.0; + double stepQuality = 0.0; + double xNorm2 = 0.0; if (solved) { + jacCostChange = calcJacCostChange(x, lmDiag); + + // x squared norm + for (int i = 0; i < nVars; i++) + { + xNorm2 += x[i] * x[i]; + } + + // undo jacobi scaling + if (jacobiScaling) + { + for (int i = 0; i < nVars; i++) + { + x[i] *= di[i]; + } + } + tempNodes = poseGraph.nodes; // Update temp nodes using x @@ -450,8 +598,17 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) // calc energy with temp nodes energy = calcEnergy(tempNodes); + costChange = oldEnergy - energy; + + stepQuality = costChange / jacCostChange; + std::cout << "#LM#" << iter; std::cout << " energy: " << energy; + std::cout << " deltaEnergy: " << costChange; + std::cout << " deltaEqEnergy: " << jacCostChange; + std::cout << " max(J^T*b): " << gradientMax; + std::cout << " norm2(x): " << xNorm2; + std::cout << " deltaEnergy/energy: " << costChange / energy; } else { @@ -460,11 +617,12 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) std::cout << std::endl; - if (!solved || (energy > oldEnergy)) + if (!solved || costChange < 0) { // failed to optimize, increase lambda and repeat lambdaLevMarq *= lmUpFactor; + lmUpFactor *= 2.0; std::cout << "LM up: " << lambdaLevMarq << ", old energy = " << oldEnergy << std::endl; } @@ -473,7 +631,12 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) // optimized successfully, decrease lambda and set variables for next iteration enough = true; - lambdaLevMarq /= lmDownFactor; + lambdaLevMarq *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + lmUpFactor = initialLmUpFactor; + + smallGradient = (gradientMax < maxGradientTolerance); + smallStep = (xNorm2 < stepNorm2Tolerance); + smallEnergyDelta = (costChange / energy < relEnergyDeltaTolerance); poseGraph.nodes = tempNodes; @@ -483,17 +646,36 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) oldEnergy = energy; - std::cout << "LM down: " << lambdaLevMarq << std::endl; + std::cout << "LM down: " << lambdaLevMarq; + std::cout << " step quality: " << stepQuality; + std::cout << std::endl; } iter++; - //TODO: fix this - done = (iter >= maxIterations); // || (startEnergy - energy) < minTolerance * startEnergy; + tooLong = (iter >= maxIterations); + + done = tooLong || smallGradient || smallStep || smallEnergyDelta; + } + } + //writePg(poseGraph, format("C:\\Temp\\g2opt\\it%03d.obj", iter)); + + } // TODO: set timers & produce report + bool found = smallGradient || smallStep || smallEnergyDelta; + + std::cout << "Finished:"; + if (!found) + std::cout << " not"; + std::cout << " found" << std::endl; + + std::cout << "smallGradient: " << (smallGradient ? "true" : "false") << std::endl; + std::cout << "smallStep: " << (smallStep ? "true" : "false") << std::endl; + std::cout << "smallEnergyDelta: " << (smallEnergyDelta ? "true" : "false") << std::endl; + std::cout << "tooLong: " << (tooLong ? "true" : "false") << std::endl; } void Optimizer::optimize(PoseGraph& poseGraph) From 7a2126c6ded8cf56a77751c63f964b4ba2b37687 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 21 Mar 2021 03:22:37 +0300 Subject: [PATCH 11/51] MyOptimize() is set to default optimizer --- modules/rgbd/src/pose_graph.cpp | 26 ++++++++++++++++---------- modules/rgbd/src/pose_graph.hpp | 2 +- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 8669b1974d2..f445845b82f 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -267,7 +267,7 @@ void writePg(const PoseGraph& pg, std::string fname) }; -void Optimizer::MyOptimize(PoseGraph& poseGraph) +void Optimizer::optimize(PoseGraph& poseGraph) { //TODO: no copying PoseGraph poseGraphOriginal = poseGraph; @@ -544,9 +544,6 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) lmDiag[i] = ld; jtj.refElem(i, i) = v + ld; - - //DEBUG - //std::cout << jtj.refElem(i, i) << " "; } std::cout << std::endl; @@ -671,14 +668,23 @@ void Optimizer::MyOptimize(PoseGraph& poseGraph) if (!found) std::cout << " not"; std::cout << " found" << std::endl; - - std::cout << "smallGradient: " << (smallGradient ? "true" : "false") << std::endl; - std::cout << "smallStep: " << (smallStep ? "true" : "false") << std::endl; - std::cout << "smallEnergyDelta: " << (smallEnergyDelta ? "true" : "false") << std::endl; - std::cout << "tooLong: " << (tooLong ? "true" : "false") << std::endl; + std::vector < std::string > txtFlags; + if (smallGradient) + txtFlags.push_back("smallGradient"); + if (smallStep) + txtFlags.push_back("smallStep"); + if (smallEnergyDelta) + txtFlags.push_back("smallEnergyDelta"); + if (tooLong) + txtFlags.push_back("tooLong"); + + std::cout << "("; + for (const auto& t : txtFlags) + std::cout << " " << t; + std::cout << ")" << std::endl; } -void Optimizer::optimize(PoseGraph& poseGraph) +void Optimizer::CeresOptimize(PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 8c84c9aa325..d4f98fc9ef2 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -262,7 +262,7 @@ class PoseGraph namespace Optimizer { -void MyOptimize(PoseGraph& poseGraph); +void CeresOptimize(PoseGraph& poseGraph); void optimize(PoseGraph& poseGraph); From 638b8d7dd2f552a5f76126270fa164da6c6b0451 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 02:00:33 +0300 Subject: [PATCH 12/51] Ceres thrown away, PoseGraph class and header/source code reorganized --- modules/rgbd/CMakeLists.txt | 12 - modules/rgbd/src/large_kinfu.cpp | 3 +- modules/rgbd/src/pose_graph.cpp | 493 +++++++++++++---------- modules/rgbd/src/pose_graph.hpp | 338 +--------------- modules/rgbd/src/sparse_block_matrix.hpp | 9 +- 5 files changed, 307 insertions(+), 548 deletions(-) diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 143cdf913af..edbb578cb2a 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,15 +1,3 @@ set(the_description "RGBD algorithms") -find_package(Ceres QUIET) ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) - -if(Ceres_FOUND) - ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND) - ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) - if(Ceres_VERSION VERSION_LESS 2.0.0) - ocv_include_directories("${CERES_INCLUDE_DIRS}") - endif() - add_definitions(/DGLOG_NO_ABBREVIATED_SEVERITIES) # avoid ERROR macro conflict in glog (ceres dependency) -else() - message(STATUS "rgbd: CERES support is disabled. Ceres Solver is Required for Posegraph optimization") -endif() diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 16006713c53..f02a499be95 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -208,6 +208,7 @@ bool LargeKinfuImpl::update(InputArray _depth) } } + template bool LargeKinfuImpl::updateT(const MatType& _depth) { @@ -279,7 +280,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) // TODO: Convert constraints to posegraph PoseGraph poseGraph = submapMgr->MapToPoseGraph(); std::cout << "Created posegraph\n"; - Optimizer::optimize(poseGraph); + poseGraph.optimize(); submapMgr->PoseGraphToMap(poseGraph); } diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index f445845b82f..9f9fe280293 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -10,14 +10,42 @@ #include #include -#if defined(CERES_FOUND) -#include -#endif namespace cv { namespace kinfu { + +// Cholesky decomposition of symmetrical 6x6 matrix +static inline cv::Matx66d llt6(Matx66d m) +{ + Matx66d L; + for (int i = 0; i < 6; i++) + { + for (int j = 0; j < (i + 1); j++) + { + double sum = 0; + for (int k = 0; k < j; k++) + sum += L(i, k) * L(j, k); + + if (i == j) + L(i, i) = sqrt(m(i, i) - sum); + else + L(i, j) = (1.0 / L(j, j) * (m(i, j) - sum)); + } + } + return L; +} + +PoseGraphEdge::PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information) : + sourceNodeId(_sourceNodeId), + targetNodeId(_targetNodeId), + pose(_transformation.rotation(), _transformation.translation()), + information(_information), + sqrtInfo(llt6(_information)) +{ } + bool PoseGraph::isValid() const { int numNodes = getNumNodes(); @@ -36,7 +64,6 @@ bool PoseGraph::isValid() const { int currNodeId = nodesToVisit.back(); nodesToVisit.pop_back(); - //std::cout << "Visiting node: " << currNodeId << "\n"; nodesVisited.insert(currNodeId); // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) @@ -54,8 +81,6 @@ bool PoseGraph::isValid() const } if (nextNodeId != -1) { - //std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) - // << std::endl; if (nodesVisited.count(nextNodeId) == 0) { nodesToVisit.push_back(nextNodeId); @@ -65,8 +90,8 @@ bool PoseGraph::isValid() const } isGraphConnected = (int(nodesVisited.size()) == numNodes); - std::cout << "nodesVisited: " << nodesVisited.size() - << " IsGraphConnected: " << isGraphConnected << std::endl; + //std::cout << "nodesVisited: " << nodesVisited.size(); + //std::cout << " IsGraphConnected: " << isGraphConnected << std::endl; bool invalidEdgeNode = false; for (int i = 0; i < numEdges; i++) { @@ -82,21 +107,27 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } - -// Taken from Ceres pose graph demo: https://ceres-solver.org/ PoseGraph::PoseGraph(const std::string& g2oFileName) : nodes(), edges() { - auto readAffine = [](std::istream& input) -> Affine3d - { - Vec3d p; - Vec4d q; - input >> p[0] >> p[1] >> p[2]; - input >> q[1] >> q[2] >> q[3] >> q[0]; - // Normalize the quaternion to account for precision loss due to - // serialization. - return Affine3d(Quatd(q).toRotMat3x3(), p); - }; + readG2OFile(g2oFileName); +} + +static Affine3d readAffine(std::istream& input) +{ + Vec3d p; + Vec4d q; + input >> p[0] >> p[1] >> p[2]; + input >> q[1] >> q[2] >> q[3] >> q[0]; + // Normalize the quaternion to account for precision loss due to + // serialization. + return Affine3d(Quatd(q).toRotMat3x3(), p); +}; + +// Rewritten from Ceres pose graph demo: https://ceres-solver.org/ +void PoseGraph::readG2OFile(const std::string& g2oFileName) +{ + nodes.clear(); edges.clear(); // for debugging purposes int minId = 0, maxId = 1 << 30; @@ -172,120 +203,258 @@ PoseGraph::PoseGraph(const std::string& g2oFileName) : } - -#if defined(CERES_FOUND) && defined(HAVE_EIGEN) - -class MyQuaternionParameterization - : public ceres::LocalParameterization { -public: - virtual ~MyQuaternionParameterization() {} - bool Plus(const double* x_ptr, const double* delta_ptr, double* x_plus_delta_ptr) const override +// Writes edge-only model of how nodes are located in space +void PoseGraph::writeToObjFile(const std::string& fname) const +{ + std::fstream of(fname, std::fstream::out); + for (const auto& n : nodes) { - Vec4d vx(x_ptr); - Quatd x(vx); - Vec3d delta(delta_ptr); - - Quatd x_plus_delta = Quatd(0, delta[0], delta[1], delta[2]).exp() * x; + Point3d d = n.second.getPose().translation(); + of << "v " << d.x << " " << d.y << " " << d.z << std::endl; + } + for (const auto& e : edges) + { + int sid = e.sourceNodeId, tid = e.targetNodeId; + of << "l " << sid + 1 << " " << tid + 1 << std::endl; + } + of.close(); +}; - *(Vec4d*)(x_plus_delta_ptr) = x_plus_delta.toVec(); +////////////////////////// +// Optimization itself // +//////////////////////// - return true; - } +// matrix form of Im(a) +const Matx44d M_Im{ 0, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 }; - bool ComputeJacobian(const double* x, double* jacobian) const override - { - Vec4d vx(x); - Matx43d jm = Optimizer::expQuatJacobian(Quatd(vx)); +// matrix form of conjugation +const Matx44d M_Conj{ 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0, + 0, 0, 0, -1 }; - for (int ii = 0; ii < 12; ii++) - jacobian[ii] = jm.val[ii]; +// matrix form of quaternion multiplication from left side +static inline Matx44d m_left(Quatd q) +{ + // M_left(a)* V(b) = + // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 + // av | 0_3] + 0_3x1 | skew(av)]) * V(b) + + float w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, -z, y, + y, z, w, -x, + z, -y, x, w }; +} - return true; - } +// matrix form of quaternion multiplication from right side +static inline Matx44d m_right(Quatd q) +{ + // M_right(b)* V(a) = + // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 + // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) + + float w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, z, -y, + y, -z, w, x, + z, y, -x, w }; +} - int GlobalSize() const override { return 4; } - int LocalSize() const override { return 3; } -}; +static inline Matx43d expQuatJacobian(Quatd q) +{ + double w = q.w, x = q.x, y = q.y, z = q.z; + return Matx43d(-x, -y, -z, + w, z, -y, + -z, w, x, + y, -x, w); +} -void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) +// concatenate matrices vertically +template static inline +Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) { - int numEdges = poseGraph.getNumEdges(); - if (numEdges == 0) + Matx<_Tp, m + k, n> res; + for (int i = 0; i < m; i++) { - CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); - return; + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } } + for (int i = 0; i < k; i++) + { + for (int j = 0; j < n; j++) + { + res(m + i, j) = b(i, j); + } + } + return res; +} - ceres::LossFunction* lossFunction = nullptr; - // TODO: Experiment with SE3 parameterization - ceres::LocalParameterization* quatLocalParameterization = - new MyQuaternionParameterization; +// concatenate matrices horizontally +template static inline +Matx<_Tp, m, n + k> concatHor(const Matx<_Tp, m, n>& a, const Matx<_Tp, m, k>& b) +{ + Matx<_Tp, m, n + k> res; - for (const PoseGraphEdge& currEdge : poseGraph.edges) + for (int i = 0; i < m; i++) { - int sourceNodeId = currEdge.getSourceNodeId(); - int targetNodeId = currEdge.getTargetNodeId(); - Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; - Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; - - ceres::CostFunction* costFunction = Pose3dAnalyticCostFunction::create(Vec3d(currEdge.pose.t), currEdge.pose.getQuat(), currEdge.information); - - problem.AddResidualBlock(costFunction, lossFunction, - sourcePose.t.val, sourcePose.vq.val, - targetPose.t.val, targetPose.vq.val); - problem.SetParameterization(sourcePose.vq.val, quatLocalParameterization); - problem.SetParameterization(targetPose.vq.val, quatLocalParameterization); + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } } - - for (const auto& it : poseGraph.nodes) + for (int i = 0; i < m; i++) { - const PoseGraphNode& node = it.second; - if (node.isPoseFixed()) + for (int j = 0; j < k; j++) { - problem.SetParameterBlockConstant(node.se3Pose.t.val); - problem.SetParameterBlockConstant(node.se3Pose.vq.val); + res(i, n + j) = b(i, j); } } + return res; } -#endif -//DEBUG -void writePg(const PoseGraph& pg, std::string fname) + +static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd targetQuat, Vec3d targetTrans, + Quatd rotMeasured, Vec3d transMeasured, Matx66d sqrtInfoMatrix, bool needJacobians, + Matx& sqj, Matx& stj, + Matx& tqj, Matx& ttj, + Vec6d& res) { - std::fstream of(fname, std::fstream::out); - for (const auto& n : pg.nodes) + // err_r = 2*Im(conj(rel_r) * measure_r) = 2*Im(conj(target_r) * source_r * measure_r) + // err_t = conj(source_r) * (target_t - source_t) * source_r - measure_t + + Quatd sourceQuatInv = sourceQuat.conjugate(); + Vec3d deltaTrans = targetTrans - sourceTrans; + + Quatd relativeQuat = sourceQuatInv * targetQuat; + Vec3d relativeTrans = sourceQuatInv.toRotMat3x3(cv::QUAT_ASSUME_UNIT) * deltaTrans; + + //! Definition should actually be relativeQuat * rotMeasured.conjugate() + Quatd deltaRot = relativeQuat.conjugate() * rotMeasured; + + Vec3d terr = relativeTrans - transMeasured; + Vec3d rerr = 2.0 * Vec3d(deltaRot.x, deltaRot.y, deltaRot.z); + Vec6d rterr(terr[0], terr[1], terr[2], rerr[0], rerr[1], rerr[2]); + + res = sqrtInfoMatrix * rterr; + + if (needJacobians) { - Point3d d = n.second.getPose().translation(); - of << "v " << d.x << " " << d.y << " " << d.z << std::endl; + // d(err_r) = 2*Im(d(conj(target_r) * source_r * measure_r)) = < measure_r is constant > = + // 2*Im((conj(d(target_r)) * source_r + conj(target_r) * d(source_r)) * measure_r) + // d(target_r) == 0: + // # d(err_r) = 2*Im(conj(target_r) * d(source_r) * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) * V(d(source_r)) + // # d(err_r) / d(source_r) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) + Matx34d drdsq = 2.0 * (m_right(rotMeasured) * m_left(targetQuat.conjugate())).get_minor<3, 4>(1, 0); + + // d(source_r) == 0: + // # d(err_r) = 2*Im(conj(d(target_r)) * source_r * measure_r) + // # V(d(err_r)) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj * V(d(target_r)) + // # d(err_r) / d(target_r) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj + Matx34d drdtq = 2.0 * (m_right(sourceQuat * rotMeasured) * M_Conj).get_minor<3, 4>(1, 0); + + // d(err_t) = d(conj(source_r) * (target_t - source_t) * source_r) = + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // conj(d(source_r)) * (target_t - source_t) * source_r + + // conj(source_r) * (target_t - source_t) * d(source_r) = + // + // conj(source_r) * (d(target_t) - d(source_t)) * source_r + + // 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // d(*_t) == 0: + // # d(err_t) = 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) + // # V(d(err_t)) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) * V(d(source_r)) + // # d(err_t) / d(source_r) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) + Matx34d dtdsq = 2 * m_left(sourceQuatInv * Quatd(0, deltaTrans[0], deltaTrans[1], deltaTrans[2])).get_minor<3, 4>(1, 0); + // deltaTrans is rotated by sourceQuatInv, so the jacobian is rot matrix of sourceQuatInv by +1 or -1 + Matx33d dtdtt = sourceQuatInv.toRotMat3x3(QUAT_ASSUME_UNIT); + Matx33d dtdst = -dtdtt; + + Matx33d z; + sqj = concatVert(dtdsq, drdsq); + tqj = concatVert(Matx34d(), drdtq); + stj = concatVert(dtdst, z); + ttj = concatVert(dtdtt, z); + + stj = sqrtInfoMatrix * stj; + ttj = sqrtInfoMatrix * ttj; + sqj = sqrtInfoMatrix * sqj; + tqj = sqrtInfoMatrix * tqj; } - for (const auto& e : pg.edges) + + return res.ddot(res); +} + + +// estimate current energy +double PoseGraph::calcEnergy(const std::map& newNodes) const +{ + double totalErr = 0; + for (const auto& e : edges) { - int sid = e.sourceNodeId, tid = e.targetNodeId; - of << "l " << sid + 1 << " " << tid + 1 << std::endl; + Pose3d srcP = newNodes.at(e.getSourceNodeId()).se3Pose; + Pose3d tgtP = newNodes.at(e.getTargetNodeId()).se3Pose; + + Vec6d res; + Matx stj, ttj; + Matx sqj, tqj; + double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, + e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ false, + sqj, stj, tqj, ttj, res); + + totalErr += err; } - of.close(); + return totalErr * 0.5; }; -void Optimizer::optimize(PoseGraph& poseGraph) +// from Ceres, equation energy change: +// eq. energy = 1/2 * (residuals + J * step)^2 = +// 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) +// eq. energy change = 1/2 * residuals^2 - eq. energy = +// residuals^T * J * step + 1/2 * (J*step)^T * J * step = +// (residuals^T * J + 1/2 * step^T * J^T * J) * step = +// step^T * ((residuals^T * J)^T + 1/2 * (step^T * J^T * J)^T) = +// 1/2 * step^T * (2 * J^T * residuals + J^T * J * step) = +// 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag - LMDiag) * step) = +// 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag) * step - LMDiag * step) = +// 1/2 * step^T * (J^T * residuals - LMDiag * step) = +// 1/2 * x^T * (jtb - lmDiag^T * x) +static inline double calcJacCostChange(const std::vector& jtb, + const std::vector& x, + const std::vector& lmDiag) { - //TODO: no copying - PoseGraph poseGraphOriginal = poseGraph; + double jdiag = 0.0; + for (size_t i = 0; i < x.size(); i++) + { + jdiag += x[i] * (jtb[i] - lmDiag[i] * x[i]); + } + double costChange = jdiag * 0.5; + return costChange; +}; + - if (!poseGraphOriginal.isValid()) +void PoseGraph::optimize() +{ + if (!isValid()) { CV_Error(Error::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); return; } - int numNodes = poseGraph.getNumNodes(); - int numEdges = poseGraph.getNumEdges(); + int numNodes = getNumNodes(); + int numEdges = getNumEdges(); // Allocate indices for nodes std::vector placesIds; std::map idToPlace; - for (const auto& ni : poseGraph.nodes) + for (const auto& ni : nodes) { if (!ni.second.isPoseFixed()) { @@ -297,13 +466,13 @@ void Optimizer::optimize(PoseGraph& poseGraph) int nVarNodes = placesIds.size(); if (!nVarNodes) { - CV_Error(Error::StsBadArg, "PoseGraph has no non-constant nodes, no optimization to be done"); + std::cout << "PoseGraph contains no non-constant nodes, skipping optimization" << std::endl; return; } if (numEdges == 0) { - CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); + std::cout << "PoseGraph has no edges, no optimization to be done" << std::endl; return; } @@ -312,52 +481,8 @@ void Optimizer::optimize(PoseGraph& poseGraph) size_t nVars = nVarNodes * 6; BlockSparseMat jtj(nVarNodes); std::vector jtb(nVars); - - // estimate current energy - auto calcEnergy = [&poseGraph](const std::map& nodes) -> double - { - double totalErr = 0; - for (const auto& e : poseGraph.edges) - { - Pose3d srcP = nodes.at(e.getSourceNodeId()).se3Pose; - Pose3d tgtP = nodes.at(e.getTargetNodeId()).se3Pose; - - Vec6d res; - Matx stj, ttj; - Matx sqj, tqj; - double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, - e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ false, - sqj, stj, tqj, ttj, res); - - totalErr += err; - } - return totalErr*0.5; - }; - - // from Ceres, equation energy change: - // eq. energy = 1/2 * (residuals + J * step)^2 = - // 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) - // eq. energy change = 1/2 * residuals^2 - eq. energy = - // residuals^T * J * step + 1/2 * (J*step)^T * J * step = - // (residuals^T * J + 1/2 * step^T * J^T * J) * step = - // step^T * ((residuals^T * J)^T + 1/2 * (step^T * J^T * J)^T) = - // 1/2 * step^T * (2 * J^T * residuals + J^T * J * step) = - // 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag - LMDiag) * step) = - // 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag) * step - LMDiag * step) = - // 1/2 * step^T * (J^T * residuals - LMDiag * step) = - // 1/2 * x^T * (jtb - lmDiag^T * x) - auto calcJacCostChange = [nVars, &jtb](const std::vector& x, const std::vector& lmDiag) -> double - { - double jdiag = 0.0; - for (int i = 0; i < nVars; i++) - { - jdiag += x[i] * (jtb[i] - lmDiag[i] * x[i]); - } - double costChange = jdiag * 0.5; - return costChange; - }; - - double energy = calcEnergy(poseGraph.nodes); + + double energy = calcEnergy(nodes); double startEnergy = energy; double oldEnergy = energy; @@ -366,10 +491,10 @@ void Optimizer::optimize(PoseGraph& poseGraph) // options // stop conditions const unsigned int maxIterations = 100; - const double maxGradientTolerance = 1e-6; + const double minGradientTolerance = 1e-6; const double stepNorm2Tolerance = 1e-6; const double relEnergyDeltaTolerance = 1e-6; - // normalize jac columns for better conditioning + // normalize jacobian columns for better conditioning const bool jacobiScaling = true; const double minDiag = 1e-6; const double maxDiag = 1e32; @@ -379,12 +504,12 @@ void Optimizer::optimize(PoseGraph& poseGraph) const double initialLmDownFactor = 3.0; // finish reasons - bool tooLong = false; // => not found - bool smallGradient = false; // => found - bool smallStep = false; // => found + bool tooLong = false; // => not found + bool smallGradient = false; // => found + bool smallStep = false; // => found bool smallEnergyDelta = false; // => found - // column scale inverted, for jacobi scaling + // column scale inverted, for jacobian scaling std::vector di(nVars); double lmUpFactor = initialLmUpFactor; @@ -402,7 +527,7 @@ void Optimizer::optimize(PoseGraph& poseGraph) std::vector> cachedJac; for (auto id : placesIds) { - Pose3d p = poseGraph.nodes.at(id).se3Pose; + Pose3d p = nodes.at(id).se3Pose; Matx43d qj = expQuatJacobian(p.getQuat()); // x node layout is (rot_x, rot_y, rot_z, trans_x, trans_y, trans_z) // pose layout is (q_w, q_x, q_y, q_z, trans_x, trans_y, trans_z) @@ -412,11 +537,11 @@ void Optimizer::optimize(PoseGraph& poseGraph) } // fill jtj and jtb - for (const auto& e : poseGraph.edges) + for (const auto& e : edges) { int srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); - const PoseGraphNode& srcNode = poseGraph.nodes.at(srcId); - const PoseGraphNode& dstNode = poseGraph.nodes.at(dstId); + const PoseGraphNode& srcNode = nodes.at(srcId); + const PoseGraphNode& dstNode = nodes.at(dstId); Pose3d srcP = srcNode.se3Pose; Pose3d tgtP = dstNode.se3Pose; @@ -528,7 +653,7 @@ void Optimizer::optimize(PoseGraph& poseGraph) // Solve using LevMarq and get delta transform bool enough = false; - decltype(poseGraph.nodes) tempNodes = poseGraph.nodes; + decltype(nodes) tempNodes = nodes; while (!enough && !done) { @@ -537,12 +662,8 @@ void Optimizer::optimize(PoseGraph& poseGraph) for (int i = 0; i < nVars; i++) { double v = diag[i]; - - //double ld = lambdaLevMarq * (v + coeffILM); double ld = std::min(max(v * lambdaLevMarq, minDiag), maxDiag); - lmDiag[i] = ld; - jtj.refElem(i, i) = v + ld; } @@ -552,9 +673,9 @@ void Optimizer::optimize(PoseGraph& poseGraph) // use double or convert everything to float std::vector x; - bool solved = kinfu::sparseSolve(jtj, Mat(jtb), x); + bool solved = kinfu::sparseSolve(jtj, jtb, x, false); - std::cout << "solve finished: " << std::endl; + std::cout << (solved ? "OK" : "FAIL") << std::endl; double costChange = 0.0; double jacCostChange = 0.0; @@ -562,7 +683,7 @@ void Optimizer::optimize(PoseGraph& poseGraph) double xNorm2 = 0.0; if (solved) { - jacCostChange = calcJacCostChange(x, lmDiag); + jacCostChange = calcJacCostChange(jtb, x, lmDiag); // x squared norm for (int i = 0; i < nVars; i++) @@ -579,7 +700,7 @@ void Optimizer::optimize(PoseGraph& poseGraph) } } - tempNodes = poseGraph.nodes; + tempNodes = nodes; // Update temp nodes using x for (int i = 0; i < nVarNodes; i++) @@ -606,13 +727,8 @@ void Optimizer::optimize(PoseGraph& poseGraph) std::cout << " max(J^T*b): " << gradientMax; std::cout << " norm2(x): " << xNorm2; std::cout << " deltaEnergy/energy: " << costChange / energy; + std::cout << std::endl; } - else - { - std::cout << "not solved" << std::endl; - } - - std::cout << std::endl; if (!solved || costChange < 0) { @@ -631,11 +747,11 @@ void Optimizer::optimize(PoseGraph& poseGraph) lambdaLevMarq *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); lmUpFactor = initialLmUpFactor; - smallGradient = (gradientMax < maxGradientTolerance); + smallGradient = (gradientMax < minGradientTolerance); smallStep = (xNorm2 < stepNorm2Tolerance); smallEnergyDelta = (costChange / energy < relEnergyDeltaTolerance); - poseGraph.nodes = tempNodes; + nodes = tempNodes; std::cout << "#" << iter; std::cout << " energy: " << energy; @@ -656,12 +772,10 @@ void Optimizer::optimize(PoseGraph& poseGraph) } } - //writePg(poseGraph, format("C:\\Temp\\g2opt\\it%03d.obj", iter)); - } - // TODO: set timers & produce report + // TODO: set timers bool found = smallGradient || smallStep || smallEnergyDelta; std::cout << "Finished:"; @@ -681,42 +795,7 @@ void Optimizer::optimize(PoseGraph& poseGraph) std::cout << "("; for (const auto& t : txtFlags) std::cout << " " << t; - std::cout << ")" << std::endl; -} - -void Optimizer::CeresOptimize(PoseGraph& poseGraph) -{ - PoseGraph poseGraphOriginal = poseGraph; - - if (!poseGraphOriginal.isValid()) - { - CV_Error(Error::StsBadArg, - "Invalid PoseGraph that is either not connected or has invalid nodes"); - return; - } - - int numNodes = poseGraph.getNumNodes(); - int numEdges = poseGraph.getNumEdges(); - std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" - << std::endl; - -#if defined(CERES_FOUND) && defined(HAVE_EIGEN) - ceres::Problem problem; - createOptimizationProblem(poseGraph, problem); - - ceres::Solver::Options options; - options.max_num_iterations = 100; - options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; - - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); - - std::cout << summary.FullReport() << '\n'; - - std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; -#else - CV_Error(Error::StsNotImplemented, "Ceres and Eigen required for Pose Graph optimization"); -#endif + std::cout << " )" << std::endl; } } // namespace kinfu diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index d4f98fc9ef2..2cdf7791360 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -11,12 +11,7 @@ #include "opencv2/core/eigen.hpp" #endif -#if defined(CERES_FOUND) -#include -#endif - #include "sparse_block_matrix.hpp" - #include "opencv2/core/dualquaternion.hpp" namespace cv @@ -29,27 +24,9 @@ namespace kinfu * Detailed description */ -// Cholesky decomposition of symmetrical 6x6 matrix -inline cv::Matx66d llt6(Matx66d m) -{ - Matx66d L; - for (int i = 0; i < 6; i++) - { - for (int j = 0; j < (i + 1); j++) - { - double sum = 0; - for (int k = 0; k < j; k++) - sum += L(i, k) * L(j, k); - - if (i == j) - L(i, i) = sqrt(m(i, i) - sum); - else - L(i, j) = (1.0 / L(j, j) * (m(i, j) - sum)); - } - } - return L; -} + +// TODO: put it to PoseGraph as a subtype struct Pose3d { Vec3d t; @@ -100,6 +77,7 @@ struct Pose3d } }; +// TODO: put it to PoseGraph as a subtype struct PoseGraphNode { public: @@ -132,6 +110,7 @@ struct PoseGraphNode Pose3d se3Pose; }; +// TODO: put it to PoseGraph as a subtype /*! \class PoseGraphEdge * \brief Defines the constraints between two PoseGraphNodes * @@ -141,14 +120,8 @@ struct PoseGraphEdge { public: PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information = Matx66f::eye()) - : sourceNodeId(_sourceNodeId), - targetNodeId(_targetNodeId), - pose(_transformation.rotation(), _transformation.translation()), - information(_information), - sqrtInfo(llt6(_information)) - { - } + const Matx66f& _information = Matx66f::eye()); + virtual ~PoseGraphEdge() = default; int getSourceNodeId() const { return sourceNodeId; } @@ -170,48 +143,9 @@ struct PoseGraphEdge Matx66f sqrtInfo; }; -//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold -//! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator -// clang-format off -/* static const std::array generatorJacobian = { */ -/* // alpha */ -/* Matx44f(0, 0, 0, 0, */ -/* 0, 0, -1, 0, */ -/* 0, 1, 0, 0, */ -/* 0, 0, 0, 0), */ -/* // beta */ -/* Matx44f( 0, 0, 1, 0, */ -/* 0, 0, 0, 0, */ -/* -1, 0, 0, 0, */ -/* 0, 0, 0, 0), */ -/* // gamma */ -/* Matx44f(0, -1, 0, 0, */ -/* 1, 0, 0, 0, */ -/* 0, 0, 0, 0, */ -/* 0, 0, 0, 0), */ -/* // x */ -/* Matx44f(0, 0, 0, 1, */ -/* 0, 0, 0, 0, */ -/* 0, 0, 0, 0, */ -/* 0, 0, 0, 0), */ -/* // y */ -/* Matx44f(0, 0, 0, 0, */ -/* 0, 0, 0, 1, */ -/* 0, 0, 0, 0, */ -/* 0, 0, 0, 0), */ -/* // z */ -/* Matx44f(0, 0, 0, 0, */ -/* 0, 0, 0, 0, */ -/* 0, 0, 0, 1, */ -/* 0, 0, 0, 0) */ -/* }; */ -// clang-format on - class PoseGraph { public: - - explicit PoseGraph() {}; virtual ~PoseGraph() = default; @@ -221,6 +155,8 @@ class PoseGraph // can be used for debugging PoseGraph(const std::string& g2oFileName); + void readG2OFile(const std::string& g2oFileName); + void writeToObjFile(const std::string& objFname) const; void addNode(const PoseGraphNode& node) { @@ -241,11 +177,6 @@ class PoseGraph bool nodeExists(int nodeId) const { return (nodes.find(nodeId) != nodes.end()); - /* - return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { - return currNode.getId() == nodeId; - }) != nodes.end(); - */ } bool isValid() const; @@ -255,257 +186,16 @@ class PoseGraph public: - std::map nodes; - std::vector edges; -}; - -namespace Optimizer -{ - -void CeresOptimize(PoseGraph& poseGraph); - -void optimize(PoseGraph& poseGraph); - -#if defined(CERES_FOUND) -void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem); - -// matrix form of Im(a) -const Matx44d M_Im{ 0, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 }; - -// matrix form of conjugation -const Matx44d M_Conj{ 1, 0, 0, 0, - 0, -1, 0, 0, - 0, 0, -1, 0, - 0, 0, 0, -1 }; - -// matrix form of quaternion multiplication from left side -inline Matx44d m_left(Quatd q) -{ - // M_left(a)* V(b) = - // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 - // av | 0_3] + 0_3x1 | skew(av)]) * V(b) - - float w = q.w, x = q.x, y = q.y, z = q.z; - return { w, -x, -y, -z, - x, w, -z, y, - y, z, w, -x, - z, -y, x, w }; -} - -// matrix form of quaternion multiplication from right side -inline Matx44d m_right(Quatd q) -{ - // M_right(b)* V(a) = - // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 - // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) - - float w = q.w, x = q.x, y = q.y, z = q.z; - return { w, -x, -y, -z, - x, w, z, -y, - y, -z, w, x, - z, y, -x, w }; -} - -inline Matx43d expQuatJacobian(Quatd q) -{ - double w = q.w, x = q.x, y = q.y, z = q.z; - return Matx43d(-x, -y, -z, - w, z, -y, - -z, w, x, - y, -x, w); -} - - -// concatenate matrices vertically -template static inline -Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) -{ - Matx<_Tp, m + k, n> res; - for (int i = 0; i < m; i++) - { - for (int j = 0; j < n; j++) - { - res(i, j) = a(i, j); - } - } - for (int i = 0; i < k; i++) - { - for (int j = 0; j < n; j++) - { - res(m + i, j) = b(i, j); - } - } - return res; -} - -// concatenate matrices horizontally -template static inline -Matx<_Tp, m, n + k> concatHor(const Matx<_Tp, m, n>& a, const Matx<_Tp, m, k>& b) -{ - Matx<_Tp, m, n + k> res; - - for (int i = 0; i < m; i++) - { - for (int j = 0; j < n; j++) - { - res(i, j) = a(i, j); - } - } - for (int i = 0; i < m; i++) - { - for (int j = 0; j < k; j++) - { - res(i, n + j) = b(i, j); - } - } - return res; -} - - -inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd targetQuat, Vec3d targetTrans, - Quatd rotMeasured, Vec3d transMeasured, Matx66d sqrtInfoMatrix, bool needJacobians, - Matx& sqj, Matx& stj, - Matx& tqj, Matx& ttj, - Vec6d& res) -{ - // err_r = 2*Im(conj(rel_r) * measure_r) = 2*Im(conj(target_r) * source_r * measure_r) - // err_t = conj(source_r) * (target_t - source_t) * source_r - measure_t - - Quatd sourceQuatInv = sourceQuat.conjugate(); - Vec3d deltaTrans = targetTrans - sourceTrans; - - Quatd relativeQuat = sourceQuatInv * targetQuat; - Vec3d relativeTrans = sourceQuatInv.toRotMat3x3(cv::QUAT_ASSUME_UNIT) * deltaTrans; - - //! Definition should actually be relativeQuat * rotMeasured.conjugate() - Quatd deltaRot = relativeQuat.conjugate() * rotMeasured; - - Vec3d terr = relativeTrans - transMeasured; - Vec3d rerr = 2.0 * Vec3d(deltaRot.x, deltaRot.y, deltaRot.z); - Vec6d rterr(terr[0], terr[1], terr[2], rerr[0], rerr[1], rerr[2]); - - res = sqrtInfoMatrix * rterr; - - if (needJacobians) - { - // d(err_r) = 2*Im(d(conj(target_r) * source_r * measure_r)) = < measure_r is constant > = - // 2*Im((conj(d(target_r)) * source_r + conj(target_r) * d(source_r)) * measure_r) - // d(target_r) == 0: - // # d(err_r) = 2*Im(conj(target_r) * d(source_r) * measure_r) - // # V(d(err_r)) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) * V(d(source_r)) - // # d(err_r) / d(source_r) = 2 * M_Im * M_right(measure_r) * M_left(conj(target_r)) - Matx34d drdsq = 2.0 * (m_right(rotMeasured) * m_left(targetQuat.conjugate())).get_minor<3, 4>(1, 0); - - // d(source_r) == 0: - // # d(err_r) = 2*Im(conj(d(target_r)) * source_r * measure_r) - // # V(d(err_r)) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj * V(d(target_r)) - // # d(err_r) / d(target_r) = 2 * M_Im * M_right(source_r * measure_r) * M_Conj - Matx34d drdtq = 2.0 * (m_right(sourceQuat * rotMeasured) * M_Conj).get_minor<3, 4>(1, 0); - - // d(err_t) = d(conj(source_r) * (target_t - source_t) * source_r) = - // conj(source_r) * (d(target_t) - d(source_t)) * source_r + - // conj(d(source_r)) * (target_t - source_t) * source_r + - // conj(source_r) * (target_t - source_t) * d(source_r) = - // - // conj(source_r) * (d(target_t) - d(source_t)) * source_r + - // 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) - // d(*_t) == 0: - // # d(err_t) = 2 * Im(conj(source_r) * (target_t - source_t) * d(source_r)) - // # V(d(err_t)) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) * V(d(source_r)) - // # d(err_t) / d(source_r) = 2 * M_Im * M_left(conj(source_r) * (target_t - source_t)) - Matx34d dtdsq = 2 * m_left(sourceQuatInv * Quatd(0, deltaTrans[0], deltaTrans[1], deltaTrans[2])).get_minor<3, 4>(1, 0); - // deltaTrans is rotated by sourceQuatInv, so the jacobian is rot matrix of sourceQuatInv by +1 or -1 - Matx33d dtdtt = sourceQuatInv.toRotMat3x3(QUAT_ASSUME_UNIT); - Matx33d dtdst = -dtdtt; - - Matx33d z; - sqj = concatVert(dtdsq, drdsq); - tqj = concatVert(Matx34d(), drdtq); - stj = concatVert(dtdst, z); - ttj = concatVert(dtdtt, z); - - stj = sqrtInfoMatrix * stj; - ttj = sqrtInfoMatrix * ttj; - sqj = sqrtInfoMatrix * sqj; - tqj = sqrtInfoMatrix * tqj; - } - - return res.ddot(res); -} + void optimize(); + // used during optimization + // nodes is a set of parameters to be used instead of contained in the graph + double calcEnergy(const std::map& newNodes) const; -class Pose3dAnalyticCostFunction : public ceres::SizedCostFunction<6, 3, 4, 3, 4> -{ -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Pose3dAnalyticCostFunction(Vec3d _transMeasured, Quatd _rotMeasured, Matx66d _infoMatrix) : - transMeasured(_transMeasured), rotMeasured(_rotMeasured), infoMatrix(_infoMatrix), - sqrtInfoMatrix(llt6(infoMatrix)) - { } - - - bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const override - { - Vec3d sourceTrans(parameters[0]); - Vec4d p1(parameters[1]); - Quatd sourceQuat(p1); - Vec3d targetTrans(parameters[2]); - Vec4d p3(parameters[3]); - Quatd targetQuat(p3); - - Vec6d res; - Matx stj, ttj; - Matx sqj, tqj; - poseError(sourceQuat, sourceTrans, targetQuat, targetTrans, - rotMeasured, transMeasured, sqrtInfoMatrix, /* needJacobians = */ (bool)(jacobians), - sqj, stj, tqj, ttj, res); - - if (jacobians) - { - // source trans, source quat, target trans, target quat - double* ptrs[4] = { stj.val, sqj.val, ttj.val, tqj.val }; - size_t sizes[4] = { 6 * 3, 6 * 4, 6 * 3, 6 * 4 }; - - for (int k = 0; k < 4; k++) - { - if (jacobians[k]) - { - for (int i = 0; i < sizes[k]; i++) - jacobians[k][i] = ptrs[k][i]; - - } - } - } - - for (int i = 0; i < 6; i++) - residuals[i] = res[i]; - - return true; - } - - static ceres::CostFunction* create(Vec3d _transMeasured, Quatd _rotMeasured, Matx66d _infoMatrix) - { - return new Pose3dAnalyticCostFunction(_transMeasured, _rotMeasured, _infoMatrix); - } - - Vec3d transMeasured; - Quatd rotMeasured; - Matx66d infoMatrix; - Matx66d sqrtInfoMatrix; + std::map nodes; + std::vector edges; }; - -#endif - -} // namespace Optimizer - } // namespace kinfu } // namespace cv #endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 790bc725cf0..1f9402a3f4c 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -146,16 +146,17 @@ struct BlockSparseMat //! Function to solve a sparse linear system of equations HX = B //! Requires Eigen template -static bool sparseSolve(const BlockSparseMat& H, const Mat& B, - OutputArray X, OutputArray predB = cv::noArray()) +static bool sparseSolve(const BlockSparseMat& H, InputArray B, + OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) { #if defined(HAVE_EIGEN) Eigen::SparseMatrix bigA = H.toEigen(); + Mat mb = B.getMat().t(); Eigen::Matrix bigB; - cv2eigen(B, bigB); + cv2eigen(mb, bigB); Eigen::SparseMatrix bigAtranspose = bigA.transpose(); - if(!bigA.isApprox(bigAtranspose)) + if(checkSymmetry && !bigA.isApprox(bigAtranspose)) { CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); return false; From 91b0b279ffbbc6e52a90cd895cf13857803d004f Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 02:39:26 +0300 Subject: [PATCH 13/51] pose, node, edge -> nested for PoseGraph --- modules/rgbd/src/pose_graph.cpp | 58 ++++----- modules/rgbd/src/pose_graph.hpp | 215 +++++++++++++++----------------- modules/rgbd/src/submap.hpp | 9 +- 3 files changed, 130 insertions(+), 152 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 9f9fe280293..dbdceff7e0e 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -37,13 +37,12 @@ static inline cv::Matx66d llt6(Matx66d m) return L; } -PoseGraphEdge::PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information) : - sourceNodeId(_sourceNodeId), - targetNodeId(_targetNodeId), - pose(_transformation.rotation(), _transformation.translation()), - information(_information), - sqrtInfo(llt6(_information)) +PoseGraph::Edge::Edge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information) : + sourceNodeId(_sourceNodeId), + targetNodeId(_targetNodeId), + pose(_transformation.rotation(), _transformation.translation()), + sqrtInfo(llt6(_information)) { } bool PoseGraph::isValid() const @@ -68,7 +67,7 @@ bool PoseGraph::isValid() const // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) { - const PoseGraphEdge& potentialEdge = edges.at(i); + const Edge& potentialEdge = edges.at(i); int nextNodeId = -1; if (potentialEdge.getSourceNodeId() == currNodeId) @@ -95,7 +94,7 @@ bool PoseGraph::isValid() const bool invalidEdgeNode = false; for (int i = 0; i < numEdges; i++) { - const PoseGraphEdge& edge = edges.at(i); + const Edge& edge = edges.at(i); // edges have spurious source/target nodes if ((nodesVisited.count(edge.getSourceNodeId()) != 1) || (nodesVisited.count(edge.getTargetNodeId()) != 1)) @@ -152,7 +151,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) if (id < minId || id >= maxId) continue; - kinfu::PoseGraphNode n(id, pose); + Node n(id, pose); if (id == minId) n.setFixed(); @@ -189,7 +188,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId)) { - edges.push_back(PoseGraphEdge(startId, endId, pose, info)); + edges.push_back(Edge(startId, endId, pose, info)); } } else @@ -392,20 +391,19 @@ static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd target // estimate current energy -double PoseGraph::calcEnergy(const std::map& newNodes) const +double PoseGraph::calcEnergy(const std::map& newNodes) const { double totalErr = 0; for (const auto& e : edges) { - Pose3d srcP = newNodes.at(e.getSourceNodeId()).se3Pose; - Pose3d tgtP = newNodes.at(e.getTargetNodeId()).se3Pose; + Pose3d srcP = newNodes.at(e.getSourceNodeId()).pose; + Pose3d tgtP = newNodes.at(e.getTargetNodeId()).pose; Vec6d res; Matx stj, ttj; Matx sqj, tqj; - double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, - e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ false, - sqj, stj, tqj, ttj, res); + double err = poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, + /* needJacobians = */ false, sqj, stj, tqj, ttj, res); totalErr += err; } @@ -463,7 +461,7 @@ void PoseGraph::optimize() } } - int nVarNodes = placesIds.size(); + size_t nVarNodes = placesIds.size(); if (!nVarNodes) { std::cout << "PoseGraph contains no non-constant nodes, skipping optimization" << std::endl; @@ -527,8 +525,8 @@ void PoseGraph::optimize() std::vector> cachedJac; for (auto id : placesIds) { - Pose3d p = nodes.at(id).se3Pose; - Matx43d qj = expQuatJacobian(p.getQuat()); + Pose3d p = nodes.at(id).pose; + Matx43d qj = expQuatJacobian(p.q); // x node layout is (rot_x, rot_y, rot_z, trans_x, trans_y, trans_z) // pose layout is (q_w, q_x, q_y, q_z, trans_x, trans_y, trans_z) Matx j = concatVert(concatHor(qj, Matx43d()), @@ -540,20 +538,19 @@ void PoseGraph::optimize() for (const auto& e : edges) { int srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); - const PoseGraphNode& srcNode = nodes.at(srcId); - const PoseGraphNode& dstNode = nodes.at(dstId); + const Node& srcNode = nodes.at(srcId); + const Node& dstNode = nodes.at(dstId); - Pose3d srcP = srcNode.se3Pose; - Pose3d tgtP = dstNode.se3Pose; + Pose3d srcP = srcNode.pose; + Pose3d tgtP = dstNode.pose; bool srcFixed = srcNode.isPoseFixed(); bool dstFixed = dstNode.isPoseFixed(); Vec6d res; Matx stj, ttj; Matx sqj, tqj; - double err = poseError(srcP.getQuat(), srcP.t, tgtP.getQuat(), tgtP.t, - e.pose.getQuat(), e.pose.t, e.sqrtInfo, /* needJacobians = */ true, - sqj, stj, tqj, ttj, res); + double err = poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, + /* needJacobians = */ true, sqj, stj, tqj, ttj, res); size_t srcPlace, dstPlace; Matx66d sj, tj; @@ -703,13 +700,13 @@ void PoseGraph::optimize() tempNodes = nodes; // Update temp nodes using x - for (int i = 0; i < nVarNodes; i++) + for (size_t i = 0; i < nVarNodes; i++) { Vec6d dx(&x[i * 6]); Vec3d deltaRot(dx[0], dx[1], dx[2]), deltaTrans(dx[3], dx[4], dx[5]); - Pose3d& p = tempNodes.at(placesIds[i]).se3Pose; + Pose3d& p = tempNodes.at(placesIds[i]).pose; - p.vq = (Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.getQuat()).toVec(); + p.q = Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.q; p.t += deltaTrans; } @@ -775,7 +772,6 @@ void PoseGraph::optimize() } - // TODO: set timers bool found = smallGradient || smallStep || smallEnergyDelta; std::cout << "Finished:"; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 2cdf7791360..7004f29bb69 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -18,133 +18,118 @@ namespace cv { namespace kinfu { -/*! \class GraphNode - * \brief Defines a node/variable that is optimizable in a posegraph - * - * Detailed description - */ - - -// TODO: put it to PoseGraph as a subtype -struct Pose3d +class PoseGraph { - Vec3d t; - Vec4d vq; - - Pose3d() : t(), vq(1, 0, 0, 0) { } - - Pose3d(const Matx33d& rotation, const Vec3d& translation) - : t(translation) +public: + struct Pose3d { - vq = Quatd::createFromRotMat(rotation).normalize().toVec(); - } - - explicit Pose3d(const Matx44d& pose) : - Pose3d(pose.get_minor<3, 3>(0, 0), Vec3d(pose(0, 3), pose(1, 3), pose(2, 3))) - { } + Vec3d t; + Quatd q; - // NOTE: Eigen overloads quaternion multiplication appropriately - inline Pose3d operator*(const Pose3d& otherPose) const - { - Pose3d out(*this); - out.t += Quatd(vq).toRotMat3x3(QUAT_ASSUME_UNIT) * otherPose.t; - out.vq = (Quatd(out.vq) * Quatd(otherPose.vq)).toVec(); - return out; - } + Pose3d() : t(), q(1, 0, 0, 0) { } - Affine3d getAffine() const - { - return Affine3d(Quatd(vq).toRotMat3x3(QUAT_ASSUME_UNIT), t); - } + Pose3d(const Matx33d& rotation, const Vec3d& translation) + : t(translation), q(Quatd::createFromRotMat(rotation).normalize()) + { } - Quatd getQuat() const - { - return Quatd(vq); - } + explicit Pose3d(const Matx44d& pose) : + Pose3d(pose.get_minor<3, 3>(0, 0), Vec3d(pose(0, 3), pose(1, 3), pose(2, 3))) + { } - inline Pose3d inverse() const - { - Pose3d out; - out.vq = Quatd(vq).conjugate().toVec(); - out.t = - (Quatd(out.vq).toRotMat3x3(QUAT_ASSUME_UNIT) * t); - return out; - } + inline Pose3d operator*(const Pose3d& otherPose) const + { + Pose3d out(*this); + out.t += q.toRotMat3x3(QUAT_ASSUME_UNIT) * otherPose.t; + out.q = out.q * otherPose.q; + return out; + } - inline void normalizeRotation() - { - vq = Quatd(vq).normalize().toVec(); - } -}; + Affine3d getAffine() const + { + return Affine3d(q.toRotMat3x3(QUAT_ASSUME_UNIT), t); + } -// TODO: put it to PoseGraph as a subtype -struct PoseGraphNode -{ - public: - explicit PoseGraphNode(int _nodeId, const Affine3d& _pose) - : nodeId(_nodeId), isFixed(false) - { - se3Pose = Pose3d(_pose.rotation(), _pose.translation()); - } - virtual ~PoseGraphNode() = default; + inline Pose3d inverse() const + { + Pose3d out; + out.q = q.conjugate(); + out.t = -(out.q.toRotMat3x3(QUAT_ASSUME_UNIT) * t); + return out; + } - int getId() const { return nodeId; } - inline Affine3d getPose() const - { - return se3Pose.getAffine(); - } - void setPose(const Affine3d& _pose) + inline void normalizeRotation() + { + q = q.normalize(); + } + }; + + /*! \class GraphNode + * \brief Defines a node/variable that is optimizable in a posegraph + * + * Detailed description + */ + struct Node { - se3Pose = Pose3d(_pose.rotation(), _pose.translation()); - } - void setPose(const Pose3d& _pose) + public: + explicit Node(int _nodeId, const Affine3d& _pose) + : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + { } + virtual ~Node() = default; + + int getId() const { return nodeId; } + inline Affine3d getPose() const + { + return pose.getAffine(); + } + void setPose(const Affine3d& _pose) + { + pose = Pose3d(_pose.rotation(), _pose.translation()); + } + void setPose(const Pose3d& _pose) + { + pose = _pose; + } + void setFixed(bool val = true) { isFixed = val; } + bool isPoseFixed() const { return isFixed; } + + public: + int nodeId; + bool isFixed; + Pose3d pose; + }; + + /*! \class PoseGraphEdge + * \brief Defines the constraints between two PoseGraphNodes + * + * Detailed description + */ + struct Edge { - se3Pose = _pose; - } - void setFixed(bool val = true) { isFixed = val; } - bool isPoseFixed() const { return isFixed; } + public: + Edge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()); - public: - int nodeId; - bool isFixed; - Pose3d se3Pose; -}; - -// TODO: put it to PoseGraph as a subtype -/*! \class PoseGraphEdge - * \brief Defines the constraints between two PoseGraphNodes - * - * Detailed description - */ -struct PoseGraphEdge -{ - public: - PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information = Matx66f::eye()); + virtual ~Edge() = default; - virtual ~PoseGraphEdge() = default; + int getSourceNodeId() const { return sourceNodeId; } + int getTargetNodeId() const { return targetNodeId; } - int getSourceNodeId() const { return sourceNodeId; } - int getTargetNodeId() const { return targetNodeId; } - - bool operator==(const PoseGraphEdge& edge) - { - if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || - (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) - return true; - return false; - } + bool operator==(const Edge& edge) + { + if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || + (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) + return true; + return false; + } - public: - int sourceNodeId; - int targetNodeId; - Pose3d pose; - Matx66f information; - Matx66f sqrtInfo; -}; + public: + int sourceNodeId; + int targetNodeId; + Pose3d pose; + Matx66f sqrtInfo; + }; -class PoseGraph -{ public: explicit PoseGraph() {}; virtual ~PoseGraph() = default; @@ -158,7 +143,7 @@ class PoseGraph void readG2OFile(const std::string& g2oFileName); void writeToObjFile(const std::string& objFname) const; - void addNode(const PoseGraphNode& node) + void addNode(const Node& node) { int id = node.getId(); const auto& it = nodes.find(id); @@ -172,7 +157,7 @@ class PoseGraph nodes.insert({ id, node }); } } - void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } + void addEdge(const Edge& edge) { edges.push_back(edge); } bool nodeExists(int nodeId) const { @@ -190,10 +175,10 @@ class PoseGraph // used during optimization // nodes is a set of parameters to be used instead of contained in the graph - double calcEnergy(const std::map& newNodes) const; + double calcEnergy(const std::map& newNodes) const; - std::map nodes; - std::vector edges; + std::map nodes; + std::vector edges; }; } // namespace kinfu diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index b40023b6a56..23f56f7bc25 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -498,7 +498,6 @@ PoseGraph SubmapManager::MapToPoseGraph() { PoseGraph localPoseGraph; - for(const auto& currSubmap : submapList) { const typename SubmapT::Constraints& constraintList = currSubmap->constraints; @@ -507,14 +506,14 @@ PoseGraph SubmapManager::MapToPoseGraph() // TODO: Handle case with duplicate constraints A -> B and B -> A /* Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); */ Matx66f informationMatrix = Matx66f::eye(); - PoseGraphEdge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); + PoseGraph::Edge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); localPoseGraph.addEdge(currEdge); } } for(const auto& currSubmap : submapList) { - PoseGraphNode currNode(currSubmap->id, currSubmap->pose); + PoseGraph::Node currNode(currSubmap->id, currSubmap->pose); if(currSubmap->id == 0) { currNode.setFixed(); @@ -522,8 +521,6 @@ PoseGraph SubmapManager::MapToPoseGraph() localPoseGraph.addNode(currNode); } - - return localPoseGraph; } @@ -532,7 +529,7 @@ void SubmapManager::PoseGraphToMap(const PoseGraph &updatedPoseGraph) { for(const auto& currSubmap : submapList) { - const PoseGraphNode& currNode = updatedPoseGraph.nodes.at(currSubmap->id); + const PoseGraph::Node& currNode = updatedPoseGraph.nodes.at(currSubmap->id); if(!currNode.isPoseFixed()) currSubmap->pose = currNode.getPose(); std::cout << "Current node: " << currSubmap->id << " Updated Pose: \n" << currSubmap->pose.matrix << std::endl; From 264ccbfcd13c85d2d8072a63f623633bf4889281 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 15:19:11 +0300 Subject: [PATCH 14/51] warnings fixed --- modules/rgbd/src/pose_graph.cpp | 52 ++++++++++++------------ modules/rgbd/src/pose_graph.hpp | 6 +-- modules/rgbd/src/sparse_block_matrix.hpp | 20 ++++++--- 3 files changed, 42 insertions(+), 36 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index dbdceff7e0e..737c624a566 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -53,15 +53,15 @@ bool PoseGraph::isValid() const if (numNodes <= 0 || numEdges <= 0) return false; - std::unordered_set nodesVisited; - std::vector nodesToVisit; + std::unordered_set nodesVisited; + std::vector nodesToVisit; nodesToVisit.push_back(nodes.begin()->first); bool isGraphConnected = false; while (!nodesToVisit.empty()) { - int currNodeId = nodesToVisit.back(); + size_t currNodeId = nodesToVisit.back(); nodesToVisit.pop_back(); nodesVisited.insert(currNodeId); // Since each node does not maintain its neighbor list @@ -136,7 +136,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) { CV_Error(cv::Error::StsError, "failed to open file"); } - + while (infile.good()) { std::string data_type; @@ -195,7 +195,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) { CV_Error(cv::Error::StsError, "unknown tag"); } - + // Clear any trailing whitespace from the line infile >> std::ws; } @@ -242,7 +242,7 @@ static inline Matx44d m_left(Quatd q) // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 // av | 0_3] + 0_3x1 | skew(av)]) * V(b) - float w = q.w, x = q.x, y = q.y, z = q.z; + double w = q.w, x = q.x, y = q.y, z = q.z; return { w, -x, -y, -z, x, w, -z, y, y, z, w, -x, @@ -256,7 +256,7 @@ static inline Matx44d m_right(Quatd q) // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) - float w = q.w, x = q.x, y = q.y, z = q.z; + double w = q.w, x = q.x, y = q.y, z = q.z; return { w, -x, -y, -z, x, w, z, -y, y, -z, w, x, @@ -391,7 +391,7 @@ static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd target // estimate current energy -double PoseGraph::calcEnergy(const std::map& newNodes) const +double PoseGraph::calcEnergy(const std::map& newNodes) const { double totalErr = 0; for (const auto& e : edges) @@ -450,8 +450,8 @@ void PoseGraph::optimize() int numEdges = getNumEdges(); // Allocate indices for nodes - std::vector placesIds; - std::map idToPlace; + std::vector placesIds; + std::map idToPlace; for (const auto& ni : nodes) { if (!ni.second.isPoseFixed()) @@ -481,7 +481,6 @@ void PoseGraph::optimize() std::vector jtb(nVars); double energy = calcEnergy(nodes); - double startEnergy = energy; double oldEnergy = energy; std::cout << "#s" << " energy: " << energy << std::endl; @@ -511,7 +510,6 @@ void PoseGraph::optimize() std::vector di(nVars); double lmUpFactor = initialLmUpFactor; - double decreaseFactorLevMarq = 2.0; double lambdaLevMarq = initialLambdaLevMarq; unsigned int iter = 0; @@ -549,10 +547,10 @@ void PoseGraph::optimize() Vec6d res; Matx stj, ttj; Matx sqj, tqj; - double err = poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, - /* needJacobians = */ true, sqj, stj, tqj, ttj, res); + poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, + /* needJacobians = */ true, sqj, stj, tqj, ttj, res); - size_t srcPlace, dstPlace; + size_t srcPlace = (size_t)(-1), dstPlace = (size_t)(-1); Matx66d sj, tj; if (!srcFixed) { @@ -572,7 +570,7 @@ void PoseGraph::optimize() { dstPlace = idToPlace.at(dstId); tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; - + jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; Vec6f jtbDst = tj.t() * res; @@ -581,7 +579,7 @@ void PoseGraph::optimize() jtb[6 * dstPlace + i] += -jtbDst[i]; } } - + if (!(srcFixed || dstFixed)) { Matx66d sjttj = sj.t() * tj; @@ -600,7 +598,7 @@ void PoseGraph::optimize() // di = { 1/(1+sqrt(d_j)) }, extra +1 to avoid div by zero if (iter == 0) { - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { double ds = sqrt(jtj.valElem(i, i)) + 1.0; di[i] = 1.0 / ds; @@ -627,7 +625,7 @@ void PoseGraph::optimize() } // scaling J^T*b - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { jtb[i] *= di[i]; } @@ -635,18 +633,18 @@ void PoseGraph::optimize() double gradientMax = 0.0; // gradient max - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { gradientMax = std::max(gradientMax, abs(jtb[i])); } // Save original diagonal of jtj matrix for LevMarq std::vector diag(nVars); - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { diag[i] = jtj.valElem(i, i); } - + // Solve using LevMarq and get delta transform bool enough = false; @@ -656,7 +654,7 @@ void PoseGraph::optimize() { // form LevMarq matrix std::vector lmDiag(nVars); - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { double v = diag[i]; double ld = std::min(max(v * lambdaLevMarq, minDiag), maxDiag); @@ -683,7 +681,7 @@ void PoseGraph::optimize() jacCostChange = calcJacCostChange(jtb, x, lmDiag); // x squared norm - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { xNorm2 += x[i] * x[i]; } @@ -691,7 +689,7 @@ void PoseGraph::optimize() // undo jacobi scaling if (jacobiScaling) { - for (int i = 0; i < nVars; i++) + for (size_t i = 0; i < nVars; i++) { x[i] *= di[i]; } @@ -705,7 +703,7 @@ void PoseGraph::optimize() Vec6d dx(&x[i * 6]); Vec3d deltaRot(dx[0], dx[1], dx[2]), deltaTrans(dx[3], dx[4], dx[5]); Pose3d& p = tempNodes.at(placesIds[i]).pose; - + p.q = Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.q; p.t += deltaTrans; } @@ -741,7 +739,7 @@ void PoseGraph::optimize() // optimized successfully, decrease lambda and set variables for next iteration enough = true; - lambdaLevMarq *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + lambdaLevMarq *= std::max(1.0 / initialLmDownFactor, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); lmUpFactor = initialLmUpFactor; smallGradient = (gradientMax < minGradientTolerance); diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 7004f29bb69..dccadf642a6 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -12,7 +12,7 @@ #endif #include "sparse_block_matrix.hpp" -#include "opencv2/core/dualquaternion.hpp" +#include "opencv2/core/quaternion.hpp" namespace cv { @@ -175,9 +175,9 @@ class PoseGraph // used during optimization // nodes is a set of parameters to be used instead of contained in the graph - double calcEnergy(const std::map& newNodes) const; + double calcEnergy(const std::map& newNodes) const; - std::map nodes; + std::map nodes; std::vector edges; }; diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 1f9402a3f4c..942b9c26b38 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -105,14 +105,14 @@ struct BlockSparseMat { int xb = ijv.first.x, yb = ijv.first.y; MatType vblock = ijv.second; - for (int i = 0; i < blockM; i++) + for (size_t i = 0; i < blockM; i++) { - for (int j = 0; j < blockN; j++) + for (size_t j = 0; j < blockN; j++) { - _Tp val = vblock(i, j); + _Tp val = vblock((int)i, (int)j); if (abs(val) >= NON_ZERO_VAL_THRESHOLD) { - tripletList.push_back(Eigen::Triplet<_Tp>(blockM * xb + i, blockN * yb + j, val)); + tripletList.push_back(Eigen::Triplet<_Tp>((int)(blockM * xb + i), (int)(blockN * yb + j), val)); } } } @@ -143,13 +143,14 @@ struct BlockSparseMat IDtoBlockValueMap ijValue; }; +#if defined(HAVE_EIGEN) //! Function to solve a sparse linear system of equations HX = B //! Requires Eigen template static bool sparseSolve(const BlockSparseMat& H, InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) { -#if defined(HAVE_EIGEN) + Eigen::SparseMatrix bigA = H.toEigen(); Mat mb = B.getMat().t(); Eigen::Matrix bigB; @@ -189,10 +190,17 @@ static bool sparseSolve(const BlockSparseMat& H, InputArray B, return true; } } + +} #else +template +static bool sparseSolve(const BlockSparseMat& /*H*/, InputArray /*B*/, + OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) +{ std::cout << "no eigen library" << std::endl; CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); -#endif } +#endif + } // namespace kinfu } // namespace cv From c3bb3a6711fbdb1f2cc8a0a7359d9b4938bda2fc Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 16:02:38 +0300 Subject: [PATCH 15/51] jacobiScaling disabled for better performance + minors --- modules/rgbd/src/pose_graph.cpp | 71 +++++++++++++++++---------------- 1 file changed, 37 insertions(+), 34 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 737c624a566..1489e4c0616 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -437,6 +437,34 @@ static inline double calcJacCostChange(const std::vector& jtb, }; +// J := J * d_inv, d_inv = make_diag(di) +// J^T*J := (J * d_inv)^T * J * d_inv = diag(di)* (J^T * J)* diag(di) = eltwise_mul(J^T*J, di*di^T) +// J^T*b := (J * d_inv)^T * b = d_inv^T * J^T*b = eltwise_mul(J^T*b, di) +static inline void doJacobiScaling(BlockSparseMat& jtj, std::vector& jtb, const std::vector& di) +{ + // scaling J^T*J + for (auto& ijv : jtj.ijValue) + { + Point2i bpt = ijv.first; + Matx66d& m = ijv.second; + for (int i = 0; i < 6; i++) + { + for (int j = 0; j < 6; j++) + { + Point2i pt(bpt.x * 6 + i, bpt.y * 6 + j); + m(i, j) *= di[pt.x] * di[pt.y]; + } + } + } + + // scaling J^T*b + for (size_t i = 0; i < di.size(); i++) + { + jtb[i] *= di[i]; + } +} + + void PoseGraph::optimize() { if (!isValid()) @@ -492,7 +520,8 @@ void PoseGraph::optimize() const double stepNorm2Tolerance = 1e-6; const double relEnergyDeltaTolerance = 1e-6; // normalize jacobian columns for better conditioning - const bool jacobiScaling = true; + // slows down sparse solver, but maybe this'd be useful for some other solver + const bool jacobiScaling = false; const double minDiag = 1e-6; const double maxDiag = 1e32; @@ -605,30 +634,7 @@ void PoseGraph::optimize() } } - // J := J * d_inv, d_inv = make_diag(di) - // J^T*J := (J * d_inv)^T * J * d_inv = diag(di)* (J^T * J)* diag(di) = eltwise_mul(J^T*J, di*di^T) - // J^T*b := (J * d_inv)^T * b = d_inv^T * J^T*b = eltwise_mul(J^T*b, di) - - // scaling J^T*J - for (auto& ijv : jtj.ijValue) - { - Point2i bpt = ijv.first; - Matx66d& m = ijv.second; - for (int i = 0; i < 6; i++) - { - for (int j = 0; j < 6; j++) - { - Point2i pt(bpt.x * 6 + i, bpt.y * 6 + j); - m(i, j) *= di[pt.x] * di[pt.y]; - } - } - } - - // scaling J^T*b - for (size_t i = 0; i < nVars; i++) - { - jtb[i] *= di[i]; - } + doJacobiScaling(jtj, jtb, di); } double gradientMax = 0.0; @@ -646,11 +652,11 @@ void PoseGraph::optimize() } // Solve using LevMarq and get delta transform - bool enough = false; + bool enoughLm = false; decltype(nodes) tempNodes = nodes; - while (!enough && !done) + while (!enoughLm && !done) { // form LevMarq matrix std::vector lmDiag(nVars); @@ -737,7 +743,7 @@ void PoseGraph::optimize() else { // optimized successfully, decrease lambda and set variables for next iteration - enough = true; + enoughLm = true; lambdaLevMarq *= std::max(1.0 / initialLmDownFactor, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); lmUpFactor = initialLmUpFactor; @@ -763,20 +769,17 @@ void PoseGraph::optimize() tooLong = (iter >= maxIterations); - done = tooLong || smallGradient || smallStep || smallEnergyDelta; + done = (tooLong || smallGradient || smallStep || smallEnergyDelta); } - - } - } - bool found = smallGradient || smallStep || smallEnergyDelta; + bool found = (smallGradient || smallStep || smallEnergyDelta); std::cout << "Finished:"; if (!found) std::cout << " not"; std::cout << " found" << std::endl; - std::vector < std::string > txtFlags; + std::vector txtFlags; if (smallGradient) txtFlags.push_back("smallGradient"); if (smallStep) From 512b40f1603a9c11e0c5f960fff309e01f2f161a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 16:10:05 +0300 Subject: [PATCH 16/51] trailing whitespace fixed --- modules/rgbd/src/pose_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 1489e4c0616..7ced9e90442 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -507,7 +507,7 @@ void PoseGraph::optimize() size_t nVars = nVarNodes * 6; BlockSparseMat jtj(nVarNodes); std::vector jtb(nVars); - + double energy = calcEnergy(nodes); double oldEnergy = energy; From 155e3eb46caa2ec0a51379a16ebef3629aa8ee0c Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 22 Mar 2021 16:39:55 +0300 Subject: [PATCH 17/51] more warnings fixed --- modules/rgbd/src/pose_graph.cpp | 30 +++++++++++++++--------------- modules/rgbd/src/pose_graph.hpp | 24 ++++++++++++------------ 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 7ced9e90442..de7562203f3 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -37,7 +37,7 @@ static inline cv::Matx66d llt6(Matx66d m) return L; } -PoseGraph::Edge::Edge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, +PoseGraph::Edge::Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, const Matx66f& _information) : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), @@ -47,10 +47,10 @@ PoseGraph::Edge::Edge(int _sourceNodeId, int _targetNodeId, const Affine3f& _tra bool PoseGraph::isValid() const { - int numNodes = getNumNodes(); - int numEdges = getNumEdges(); + size_t numNodes = getNumNodes(); + size_t numEdges = getNumEdges(); - if (numNodes <= 0 || numEdges <= 0) + if (!numNodes || !numEdges) return false; std::unordered_set nodesVisited; @@ -65,10 +65,10 @@ bool PoseGraph::isValid() const nodesToVisit.pop_back(); nodesVisited.insert(currNodeId); // Since each node does not maintain its neighbor list - for (int i = 0; i < numEdges; i++) + for (size_t i = 0; i < numEdges; i++) { const Edge& potentialEdge = edges.at(i); - int nextNodeId = -1; + size_t nextNodeId = (size_t)(-1); if (potentialEdge.getSourceNodeId() == currNodeId) { @@ -78,7 +78,7 @@ bool PoseGraph::isValid() const { nextNodeId = potentialEdge.getSourceNodeId(); } - if (nextNodeId != -1) + if (nextNodeId != (size_t)(-1)) { if (nodesVisited.count(nextNodeId) == 0) { @@ -88,11 +88,11 @@ bool PoseGraph::isValid() const } } - isGraphConnected = (int(nodesVisited.size()) == numNodes); + isGraphConnected = (nodesVisited.size() == numNodes); //std::cout << "nodesVisited: " << nodesVisited.size(); //std::cout << " IsGraphConnected: " << isGraphConnected << std::endl; bool invalidEdgeNode = false; - for (int i = 0; i < numEdges; i++) + for (size_t i = 0; i < numEdges; i++) { const Edge& edge = edges.at(i); // edges have spurious source/target nodes @@ -144,7 +144,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) infile >> data_type; if (data_type == "VERTEX_SE3:QUAT") { - int id; + size_t id; infile >> id; Affine3d pose = readAffine(infile); @@ -169,7 +169,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) } else if (data_type == "EDGE_SE3:QUAT") { - int startId, endId; + size_t startId, endId; infile >> startId >> endId; Affine3d pose = readAffine(infile); @@ -213,7 +213,7 @@ void PoseGraph::writeToObjFile(const std::string& fname) const } for (const auto& e : edges) { - int sid = e.sourceNodeId, tid = e.targetNodeId; + size_t sid = e.sourceNodeId, tid = e.targetNodeId; of << "l " << sid + 1 << " " << tid + 1 << std::endl; } of.close(); @@ -474,8 +474,8 @@ void PoseGraph::optimize() return; } - int numNodes = getNumNodes(); - int numEdges = getNumEdges(); + size_t numNodes = getNumNodes(); + size_t numEdges = getNumEdges(); // Allocate indices for nodes std::vector placesIds; @@ -564,7 +564,7 @@ void PoseGraph::optimize() // fill jtj and jtb for (const auto& e : edges) { - int srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); + size_t srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); const Node& srcNode = nodes.at(srcId); const Node& dstNode = nodes.at(dstId); diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index dccadf642a6..3e628c4dfa0 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -72,12 +72,12 @@ class PoseGraph struct Node { public: - explicit Node(int _nodeId, const Affine3d& _pose) + explicit Node(size_t _nodeId, const Affine3d& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) { } virtual ~Node() = default; - int getId() const { return nodeId; } + size_t getId() const { return nodeId; } inline Affine3d getPose() const { return pose.getAffine(); @@ -94,7 +94,7 @@ class PoseGraph bool isPoseFixed() const { return isFixed; } public: - int nodeId; + size_t nodeId; bool isFixed; Pose3d pose; }; @@ -107,13 +107,13 @@ class PoseGraph struct Edge { public: - Edge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, + Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, const Matx66f& _information = Matx66f::eye()); virtual ~Edge() = default; - int getSourceNodeId() const { return sourceNodeId; } - int getTargetNodeId() const { return targetNodeId; } + size_t getSourceNodeId() const { return sourceNodeId; } + size_t getTargetNodeId() const { return targetNodeId; } bool operator==(const Edge& edge) { @@ -124,8 +124,8 @@ class PoseGraph } public: - int sourceNodeId; - int targetNodeId; + size_t sourceNodeId; + size_t targetNodeId; Pose3d pose; Matx66f sqrtInfo; }; @@ -145,7 +145,7 @@ class PoseGraph void addNode(const Node& node) { - int id = node.getId(); + size_t id = node.getId(); const auto& it = nodes.find(id); if (it != nodes.end()) { @@ -159,15 +159,15 @@ class PoseGraph } void addEdge(const Edge& edge) { edges.push_back(edge); } - bool nodeExists(int nodeId) const + bool nodeExists(size_t nodeId) const { return (nodes.find(nodeId) != nodes.end()); } bool isValid() const; - int getNumNodes() const { return int(nodes.size()); } - int getNumEdges() const { return int(edges.size()); } + size_t getNumNodes() const { return nodes.size(); } + size_t getNumEdges() const { return edges.size(); } public: From e90100fbe665c4339f9f570f3520faf1519d1282 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 23 Mar 2021 01:20:26 +0300 Subject: [PATCH 18/51] message added: Eigen is required for build + minors --- modules/rgbd/CMakeLists.txt | 4 ++++ modules/rgbd/src/pose_graph.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index edbb578cb2a..b0a7c73294e 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,3 +1,7 @@ set(the_description "RGBD algorithms") ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) + +if(NOT HAVE_EIGEN) + message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization") +endif() diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index de7562203f3..b8680f59abe 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -129,7 +129,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) nodes.clear(); edges.clear(); // for debugging purposes - int minId = 0, maxId = 1 << 30; + size_t minId = 0, maxId = 1 << 30; std::ifstream infile(g2oFileName.c_str()); if (!infile) From 7c1a20543ccf37fd53b16aea0ed280bc91e1f218 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 23 Mar 2021 02:09:38 +0300 Subject: [PATCH 19/51] trying to fix warning --- modules/rgbd/src/pose_graph.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index b8680f59abe..eb67aae076a 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -467,6 +467,10 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto void PoseGraph::optimize() { +#if !defined(HAVE_EIGEN) + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); +#endif + if (!isValid()) { CV_Error(Error::StsBadArg, From 87c959f1cecc012746b4c1bac2c6748443eb040d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 23 Mar 2021 14:27:04 +0300 Subject: [PATCH 20/51] try to fix "unreachable code" warning --- modules/rgbd/src/pose_graph.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index eb67aae076a..64164dec5b5 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -467,9 +467,7 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto void PoseGraph::optimize() { -#if !defined(HAVE_EIGEN) - CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); -#endif + #if defined(HAVE_EIGEN) if (!isValid()) { @@ -797,6 +795,10 @@ void PoseGraph::optimize() for (const auto& t : txtFlags) std::cout << " " << t; std::cout << " )" << std::endl; + +#else + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); +#endif } } // namespace kinfu From 2d5bd7e75780dc1838dbd65c730f883d5fa4a59c Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 23 Mar 2021 15:39:08 +0300 Subject: [PATCH 21/51] trying to fix unreachable code, pt.3 --- modules/rgbd/src/pose_graph.cpp | 8 +-- modules/rgbd/src/sparse_block_matrix.hpp | 88 +++++++++++------------- 2 files changed, 42 insertions(+), 54 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 64164dec5b5..96be697fc90 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -467,8 +467,6 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto void PoseGraph::optimize() { - #if defined(HAVE_EIGEN) - if (!isValid()) { CV_Error(Error::StsBadArg, @@ -676,7 +674,7 @@ void PoseGraph::optimize() // use double or convert everything to float std::vector x; - bool solved = kinfu::sparseSolve(jtj, jtb, x, false); + bool solved = jtj.sparseSolve(jtb, x, false); std::cout << (solved ? "OK" : "FAIL") << std::endl; @@ -795,10 +793,6 @@ void PoseGraph::optimize() for (const auto& t : txtFlags) std::cout << " " << t; std::cout << " )" << std::endl; - -#else - CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); -#endif } } // namespace kinfu diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 942b9c26b38..7851bde2a17 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -138,69 +138,63 @@ struct BlockSparseMat return *this; } - static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001); - size_t nBlocks; - IDtoBlockValueMap ijValue; -}; - #if defined(HAVE_EIGEN) -//! Function to solve a sparse linear system of equations HX = B -//! Requires Eigen -template -static bool sparseSolve(const BlockSparseMat& H, InputArray B, - OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) -{ - - Eigen::SparseMatrix bigA = H.toEigen(); - Mat mb = B.getMat().t(); - Eigen::Matrix bigB; - cv2eigen(mb, bigB); - - Eigen::SparseMatrix bigAtranspose = bigA.transpose(); - if(checkSymmetry && !bigA.isApprox(bigAtranspose)) + //! Function to solve a sparse linear system of equations HX = B + //! Requires Eigen + bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const { - CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); - return false; - } + Eigen::SparseMatrix<_Tp> bigA = toEigen(); + Mat mb = B.getMat().t(); + Eigen::Matrix<_Tp, -1, 1> bigB; + cv2eigen(mb, bigB); - Eigen::SimplicialLDLT> solver; + Eigen::SparseMatrix<_Tp> bigAtranspose = bigA.transpose(); + if(checkSymmetry && !bigA.isApprox(bigAtranspose)) + { + CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); + return false; + } - solver.compute(bigA); - if (solver.info() != Eigen::Success) - { - std::cout << "failed to eigen-decompose" << std::endl; - return false; - } - else - { - Eigen::Matrix solutionX = solver.solve(bigB); + Eigen::SimplicialLDLT> solver; + + solver.compute(bigA); if (solver.info() != Eigen::Success) { - std::cout << "failed to eigen-solve" << std::endl; + std::cout << "failed to eigen-decompose" << std::endl; return false; } else { - eigen2cv(solutionX, X); - if (predB.needed()) + Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB); + if (solver.info() != Eigen::Success) { - Eigen::Matrix predBEigen = bigA * solutionX; - eigen2cv(predBEigen, predB); + std::cout << "failed to eigen-solve" << std::endl; + return false; + } + else + { + eigen2cv(solutionX, X); + if (predB.needed()) + { + Eigen::Matrix<_Tp, -1, 1> predBEigen = bigA * solutionX; + eigen2cv(predBEigen, predB); + } + return true; } - return true; } } - -} #else -template -static bool sparseSolve(const BlockSparseMat& /*H*/, InputArray /*B*/, - OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) -{ - std::cout << "no eigen library" << std::endl; - CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); -} + bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) + { + std::cout << "no eigen library" << std::endl; + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); + } #endif + static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001); + size_t nBlocks; + IDtoBlockValueMap ijValue; +}; + } // namespace kinfu } // namespace cv From 8d64f2a3625bce9f5c88c27e6fe36fe1f3e6ee23 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 23 Mar 2021 22:23:46 +0300 Subject: [PATCH 22/51] trying to fix unreachable code, pt. 5 --- modules/rgbd/src/sparse_block_matrix.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 7851bde2a17..0f647b103e1 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -188,6 +188,7 @@ struct BlockSparseMat { std::cout << "no eigen library" << std::endl; CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); + return false; } #endif From a37c1ba43677af80060f38aaa6e310486393c864 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 00:55:45 +0300 Subject: [PATCH 23/51] trying to fix unreachable code, pt. the worst + minors --- modules/rgbd/src/pose_graph.cpp | 4 ++++ modules/rgbd/src/sparse_block_matrix.hpp | 14 ++++++-------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 96be697fc90..77167123c65 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -10,6 +10,10 @@ #include #include +// a very stupid workaround against unreachable code warning +#if defined(_MSC_VER) +#pragma warning(disable : 4702) +#endif namespace cv { diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 0f647b103e1..27c60ed0bf2 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -48,7 +48,7 @@ struct BlockSparseMat ijValue.clear(); } - MatType& refBlock(size_t i, size_t j) + inline MatType& refBlock(size_t i, size_t j) { Point2i p((int)i, (int)j); auto it = ijValue.find(p); @@ -59,14 +59,14 @@ struct BlockSparseMat return it->second; } - _Tp& refElem(size_t i, size_t j) + inline _Tp& refElem(size_t i, size_t j) { Point2i ib((int)(i / blockM), (int)(j / blockN)); Point2i iv((int)(i % blockM), (int)(j % blockN)); return refBlock(ib.x, ib.y)(iv.x, iv.y); } - MatType valBlock(size_t i, size_t j) const + inline MatType valBlock(size_t i, size_t j) const { Point2i p((int)i, (int)j); auto it = ijValue.find(p); @@ -76,7 +76,7 @@ struct BlockSparseMat return it->second; } - _Tp valElem(size_t i, size_t j) const + inline _Tp valElem(size_t i, size_t j) const { Point2i ib((int)(i / blockM), (int)(j / blockN)); Point2i iv((int)(i % blockM), (int)(j % blockN)); @@ -124,7 +124,7 @@ struct BlockSparseMat return EigenMat; } #endif - size_t nonZeroBlocks() const { return ijValue.size(); } + inline size_t nonZeroBlocks() const { return ijValue.size(); } BlockSparseMat<_Tp, blockM, blockN>& operator+=(const BlockSparseMat<_Tp, blockM, blockN>& other) { @@ -184,11 +184,9 @@ struct BlockSparseMat } } #else - bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) + bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) const { - std::cout << "no eigen library" << std::endl; CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); - return false; } #endif From 54264ba385b77ced95c875c09adf078827db86cf Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 15:33:13 +0300 Subject: [PATCH 24/51] try to fix unreachable code, pt. the ugliest --- modules/rgbd/src/pose_graph.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 77167123c65..9b2986bd9d4 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -10,11 +10,6 @@ #include #include -// a very stupid workaround against unreachable code warning -#if defined(_MSC_VER) -#pragma warning(disable : 4702) -#endif - namespace cv { namespace kinfu @@ -471,6 +466,7 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto void PoseGraph::optimize() { +#if defined(HAVE_EIGEN) if (!isValid()) { CV_Error(Error::StsBadArg, @@ -797,7 +793,11 @@ void PoseGraph::optimize() for (const auto& t : txtFlags) std::cout << " " << t; std::cout << " )" << std::endl; +#else + CV_Error(Error::StsNotImplemented, "Eigen library required for sparse matrix solve during pose graph optimization, dense solver is not implemented"); +#endif } + } // namespace kinfu } // namespace cv From 25d67f8e1da86889773bd3facaf283869d25f776 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 16:26:57 +0300 Subject: [PATCH 25/51] trying to fix unreachable code, pt. the grumpiest --- modules/rgbd/src/pose_graph.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 9b2986bd9d4..b7048ccbd45 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -262,15 +262,6 @@ static inline Matx44d m_right(Quatd q) z, y, -x, w }; } -static inline Matx43d expQuatJacobian(Quatd q) -{ - double w = q.w, x = q.x, y = q.y, z = q.z; - return Matx43d(-x, -y, -z, - w, z, -y, - -z, w, x, - y, -x, w); -} - // concatenate matrices vertically template static inline Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) @@ -410,6 +401,17 @@ double PoseGraph::calcEnergy(const std::map& newNodes) const }; +#if defined(HAVE_EIGEN) + +static inline Matx43d expQuatJacobian(Quatd q) +{ + double w = q.w, x = q.x, y = q.y, z = q.z; + return Matx43d(-x, -y, -z, + w, z, -y, + -z, w, x, + y, -x, w); +} + // from Ceres, equation energy change: // eq. energy = 1/2 * (residuals + J * step)^2 = // 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) @@ -466,7 +468,6 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto void PoseGraph::optimize() { -#if defined(HAVE_EIGEN) if (!isValid()) { CV_Error(Error::StsBadArg, @@ -591,7 +592,7 @@ void PoseGraph::optimize() Vec6f jtbSrc = sj.t() * res; for (int i = 0; i < 6; i++) { - jtb[6 * srcPlace + i] += - jtbSrc[i]; + jtb[6 * srcPlace + i] += -jtbSrc[i]; } } @@ -793,10 +794,13 @@ void PoseGraph::optimize() for (const auto& t : txtFlags) std::cout << " " << t; std::cout << " )" << std::endl; +} #else +void PoseGraph::optimize() +{ CV_Error(Error::StsNotImplemented, "Eigen library required for sparse matrix solve during pose graph optimization, dense solver is not implemented"); -#endif } +#endif } // namespace kinfu From 170f1f36b8e6b848c207b5e15fb7d9b7eabfa0b4 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 22:46:18 +0300 Subject: [PATCH 26/51] cout -> CV_LOG_INFO --- modules/rgbd/src/pose_graph.cpp | 68 +++++++++--------------- modules/rgbd/src/sparse_block_matrix.hpp | 5 +- 2 files changed, 29 insertions(+), 44 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index b7048ccbd45..215b9a72a13 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -5,7 +5,6 @@ #include "pose_graph.hpp" #include -#include #include #include #include @@ -88,8 +87,9 @@ bool PoseGraph::isValid() const } isGraphConnected = (nodesVisited.size() == numNodes); - //std::cout << "nodesVisited: " << nodesVisited.size(); - //std::cout << " IsGraphConnected: " << isGraphConnected << std::endl; + + CV_LOG_INFO(NULL, "nodesVisited: " << nodesVisited.size() << " IsGraphConnected: " << isGraphConnected); + bool invalidEdgeNode = false; for (size_t i = 0; i < numEdges; i++) { @@ -158,7 +158,7 @@ void PoseGraph::readG2OFile(const std::string& g2oFileName) const auto& it = nodes.find(id); if (it != nodes.end()) { - std::cout << "duplicated node, id=" << id << std::endl; + CV_LOG_INFO(NULL, "duplicated node, id=" << id); nodes.insert(it, { id, n }); } else @@ -493,17 +493,17 @@ void PoseGraph::optimize() size_t nVarNodes = placesIds.size(); if (!nVarNodes) { - std::cout << "PoseGraph contains no non-constant nodes, skipping optimization" << std::endl; + CV_LOG_INFO(NULL, "PoseGraph contains no non-constant nodes, skipping optimization"); return; } if (numEdges == 0) { - std::cout << "PoseGraph has no edges, no optimization to be done" << std::endl; + CV_LOG_INFO(NULL, "PoseGraph has no edges, no optimization to be done"); return; } - std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; + CV_LOG_INFO(NULL, "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges"); size_t nVars = nVarNodes * 6; BlockSparseMat jtj(nVarNodes); @@ -512,7 +512,7 @@ void PoseGraph::optimize() double energy = calcEnergy(nodes); double oldEnergy = energy; - std::cout << "#s" << " energy: " << energy << std::endl; + CV_LOG_INFO(NULL, "#s" << " energy: " << energy); // options // stop conditions @@ -618,7 +618,7 @@ void PoseGraph::optimize() } } - std::cout << "#LM#s" << " energy: " << energy << std::endl; + CV_LOG_INFO(NULL, "#LM#s" << " energy: " << energy); // do the jacobian conditioning improvement used in Ceres if (jacobiScaling) @@ -669,15 +669,13 @@ void PoseGraph::optimize() jtj.refElem(i, i) = v + ld; } - std::cout << std::endl; - - std::cout << "sparse solve..."; + CV_LOG_INFO(NULL, "sparse solve..."); // use double or convert everything to float std::vector x; bool solved = jtj.sparseSolve(jtb, x, false); - std::cout << (solved ? "OK" : "FAIL") << std::endl; + CV_LOG_INFO(NULL, (solved ? "OK" : "FAIL")); double costChange = 0.0; double jacCostChange = 0.0; @@ -722,14 +720,13 @@ void PoseGraph::optimize() stepQuality = costChange / jacCostChange; - std::cout << "#LM#" << iter; - std::cout << " energy: " << energy; - std::cout << " deltaEnergy: " << costChange; - std::cout << " deltaEqEnergy: " << jacCostChange; - std::cout << " max(J^T*b): " << gradientMax; - std::cout << " norm2(x): " << xNorm2; - std::cout << " deltaEnergy/energy: " << costChange / energy; - std::cout << std::endl; + CV_LOG_INFO(NULL, "#LM#" << iter + << " energy: " << energy + << " deltaEnergy: " << costChange + << " deltaEqEnergy: " << jacCostChange + << " max(J^T*b): " << gradientMax + << " norm2(x): " << xNorm2 + << " deltaEnergy/energy: " << costChange / energy); } if (!solved || costChange < 0) @@ -739,7 +736,7 @@ void PoseGraph::optimize() lambdaLevMarq *= lmUpFactor; lmUpFactor *= 2.0; - std::cout << "LM up: " << lambdaLevMarq << ", old energy = " << oldEnergy << std::endl; + CV_LOG_INFO(NULL, "LM up: " << lambdaLevMarq << ", old energy = " << oldEnergy); } else { @@ -755,15 +752,11 @@ void PoseGraph::optimize() nodes = tempNodes; - std::cout << "#" << iter; - std::cout << " energy: " << energy; - std::cout << std::endl; + CV_LOG_INFO(NULL, "#" << iter << " energy: " << energy); oldEnergy = energy; - std::cout << "LM down: " << lambdaLevMarq; - std::cout << " step quality: " << stepQuality; - std::cout << std::endl; + CV_LOG_INFO(NULL, "LM down: " << lambdaLevMarq << " step quality: " << stepQuality); } iter++; @@ -776,24 +769,15 @@ void PoseGraph::optimize() bool found = (smallGradient || smallStep || smallEnergyDelta); - std::cout << "Finished:"; - if (!found) - std::cout << " not"; - std::cout << " found" << std::endl; - std::vector txtFlags; + CV_LOG_INFO(NULL, "Finished: " << (found ? "" : "not") << "found"); if (smallGradient) - txtFlags.push_back("smallGradient"); + CV_LOG_INFO(NULL, "Finish reason: gradient max val dropped below threshold"); if (smallStep) - txtFlags.push_back("smallStep"); + CV_LOG_INFO(NULL, "Finish reason: step size dropped below threshold"); if (smallEnergyDelta) - txtFlags.push_back("smallEnergyDelta"); + CV_LOG_INFO(NULL, "Finish reason: relative energy change between iterations dropped below threshold"); if (tooLong) - txtFlags.push_back("tooLong"); - - std::cout << "("; - for (const auto& t : txtFlags) - std::cout << " " << t; - std::cout << " )" << std::endl; + CV_LOG_INFO(NULL, "Finish reason: max number of iterations reached"); } #else void PoseGraph::optimize() diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 27c60ed0bf2..2826f948664 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -7,6 +7,7 @@ #include "opencv2/core/base.hpp" #include "opencv2/core/types.hpp" +#include "opencv2/core/utils/logger.hpp" #if defined(HAVE_EIGEN) #include @@ -160,7 +161,7 @@ struct BlockSparseMat solver.compute(bigA); if (solver.info() != Eigen::Success) { - std::cout << "failed to eigen-decompose" << std::endl; + CV_LOG_INFO(NULL, "Failed to eigen-decompose"); return false; } else @@ -168,7 +169,7 @@ struct BlockSparseMat Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB); if (solver.info() != Eigen::Success) { - std::cout << "failed to eigen-solve" << std::endl; + CV_LOG_INFO(NULL, "Failed to eigen-solve"); return false; } else From ca6eef205238728d877578fee0616959df062e16 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 22:48:51 +0300 Subject: [PATCH 27/51] quat matrix functions moved outside cv and kinfu namespaces --- modules/rgbd/src/pose_graph.cpp | 180 ++++++++++++++++---------------- 1 file changed, 88 insertions(+), 92 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 215b9a72a13..63fa4e60a5f 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -9,6 +9,94 @@ #include #include +// matrix form of conjugation +static const cv::Matx44d M_Conj{ 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0, + 0, 0, 0, -1 }; + +// matrix form of quaternion multiplication from left side +static inline cv::Matx44d m_left(cv::Quatd q) +{ + // M_left(a)* V(b) = + // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 + // av | 0_3] + 0_3x1 | skew(av)]) * V(b) + + double w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, -z, y, + y, z, w, -x, + z, -y, x, w }; +} + +// matrix form of quaternion multiplication from right side +static inline cv::Matx44d m_right(cv::Quatd q) +{ + // M_right(b)* V(a) = + // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 + // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) + + double w = q.w, x = q.x, y = q.y, z = q.z; + return { w, -x, -y, -z, + x, w, z, -y, + y, -z, w, x, + z, y, -x, w }; +} + +static inline cv::Matx43d expQuatJacobian(cv::Quatd q) +{ + double w = q.w, x = q.x, y = q.y, z = q.z; + return cv::Matx43d(-x, -y, -z, + w, z, -y, + -z, w, x, + y, -x, w); +} + +// concatenate matrices vertically +template static inline +cv::Matx<_Tp, m + k, n> concatVert(const cv::Matx<_Tp, m, n>& a, const cv::Matx<_Tp, k, n>& b) +{ + cv::Matx<_Tp, m + k, n> res; + for (int i = 0; i < m; i++) + { + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } + } + for (int i = 0; i < k; i++) + { + for (int j = 0; j < n; j++) + { + res(m + i, j) = b(i, j); + } + } + return res; +} + +// concatenate matrices horizontally +template static inline +cv::Matx<_Tp, m, n + k> concatHor(const cv::Matx<_Tp, m, n>& a, const cv::Matx<_Tp, m, k>& b) +{ + cv::Matx<_Tp, m, n + k> res; + + for (int i = 0; i < m; i++) + { + for (int j = 0; j < n; j++) + { + res(i, j) = a(i, j); + } + } + for (int i = 0; i < m; i++) + { + for (int j = 0; j < k; j++) + { + res(i, n + j) = b(i, j); + } + } + return res; +} + namespace cv { namespace kinfu @@ -222,90 +310,7 @@ void PoseGraph::writeToObjFile(const std::string& fname) const // Optimization itself // //////////////////////// -// matrix form of Im(a) -const Matx44d M_Im{ 0, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 }; -// matrix form of conjugation -const Matx44d M_Conj{ 1, 0, 0, 0, - 0, -1, 0, 0, - 0, 0, -1, 0, - 0, 0, 0, -1 }; - -// matrix form of quaternion multiplication from left side -static inline Matx44d m_left(Quatd q) -{ - // M_left(a)* V(b) = - // = (I_4 * a0 + [ 0 | -av [ 0 | 0_1x3 - // av | 0_3] + 0_3x1 | skew(av)]) * V(b) - - double w = q.w, x = q.x, y = q.y, z = q.z; - return { w, -x, -y, -z, - x, w, -z, y, - y, z, w, -x, - z, -y, x, w }; -} - -// matrix form of quaternion multiplication from right side -static inline Matx44d m_right(Quatd q) -{ - // M_right(b)* V(a) = - // = (I_4 * b0 + [ 0 | -bv [ 0 | 0_1x3 - // bv | 0_3] + 0_3x1 | skew(-bv)]) * V(a) - - double w = q.w, x = q.x, y = q.y, z = q.z; - return { w, -x, -y, -z, - x, w, z, -y, - y, -z, w, x, - z, y, -x, w }; -} - -// concatenate matrices vertically -template static inline -Matx<_Tp, m + k, n> concatVert(const Matx<_Tp, m, n>& a, const Matx<_Tp, k, n>& b) -{ - Matx<_Tp, m + k, n> res; - for (int i = 0; i < m; i++) - { - for (int j = 0; j < n; j++) - { - res(i, j) = a(i, j); - } - } - for (int i = 0; i < k; i++) - { - for (int j = 0; j < n; j++) - { - res(m + i, j) = b(i, j); - } - } - return res; -} - -// concatenate matrices horizontally -template static inline -Matx<_Tp, m, n + k> concatHor(const Matx<_Tp, m, n>& a, const Matx<_Tp, m, k>& b) -{ - Matx<_Tp, m, n + k> res; - - for (int i = 0; i < m; i++) - { - for (int j = 0; j < n; j++) - { - res(i, j) = a(i, j); - } - } - for (int i = 0; i < m; i++) - { - for (int j = 0; j < k; j++) - { - res(i, n + j) = b(i, j); - } - } - return res; -} static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd targetQuat, Vec3d targetTrans, @@ -403,15 +408,6 @@ double PoseGraph::calcEnergy(const std::map& newNodes) const #if defined(HAVE_EIGEN) -static inline Matx43d expQuatJacobian(Quatd q) -{ - double w = q.w, x = q.x, y = q.y, z = q.z; - return Matx43d(-x, -y, -z, - w, z, -y, - -z, w, x, - y, -x, w); -} - // from Ceres, equation energy change: // eq. energy = 1/2 * (residuals + J * step)^2 = // 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) From a718ade30e25a204a6322332e8b0bf4de80a3fa5 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 24 Mar 2021 23:43:23 +0300 Subject: [PATCH 28/51] unused function fix --- modules/rgbd/src/pose_graph.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 63fa4e60a5f..dcb6fde5bf4 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -43,6 +43,9 @@ static inline cv::Matx44d m_right(cv::Quatd q) z, y, -x, w }; } +// precaution against "unused function" warning when there's no Eigen +#if defined(HAVE_EIGEN) +// jacobian of quaternionic (exp(x)*q) : R_3 -> H near x == 0 static inline cv::Matx43d expQuatJacobian(cv::Quatd q) { double w = q.w, x = q.x, y = q.y, z = q.z; @@ -51,6 +54,7 @@ static inline cv::Matx43d expQuatJacobian(cv::Quatd q) -z, w, x, y, -x, w); } +#endif // concatenate matrices vertically template static inline From ae792cf8188793caddd45f7bf65c99f02298f14c Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 29 Mar 2021 00:30:32 +0300 Subject: [PATCH 29/51] pose graph made public (but in detail namespace) + test for pose graph --- modules/rgbd/include/opencv2/rgbd.hpp | 1 + .../opencv2/rgbd/detail}/pose_graph.hpp | 8 ++- modules/rgbd/test/test_pose_graph.cpp | 55 +++++++++++++++++++ 3 files changed, 62 insertions(+), 2 deletions(-) rename modules/rgbd/{src => include/opencv2/rgbd/detail}/pose_graph.hpp (97%) create mode 100644 modules/rgbd/test/test_pose_graph.cpp diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index d4ac749c2a5..550d0cc8398 100755 --- a/modules/rgbd/include/opencv2/rgbd.hpp +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -14,6 +14,7 @@ #include "opencv2/rgbd/kinfu.hpp" #include "opencv2/rgbd/dynafu.hpp" #include "opencv2/rgbd/large_kinfu.hpp" +#include "opencv2/rgbd/detail/pose_graph.hpp" /** @defgroup rgbd RGB-Depth Processing diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp similarity index 97% rename from modules/rgbd/src/pose_graph.hpp rename to modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp index 3e628c4dfa0..46495963aba 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp @@ -18,7 +18,10 @@ namespace cv { namespace kinfu { +namespace detail +{ +// TODO: text about unstable API class PoseGraph { public: @@ -171,16 +174,17 @@ class PoseGraph public: - void optimize(); + void optimize(const cv::TermCriteria& tc); // used during optimization // nodes is a set of parameters to be used instead of contained in the graph double calcEnergy(const std::map& newNodes) const; +//TODO: pImpl std::map nodes; std::vector edges; }; - +} // namespace detail } // namespace kinfu } // namespace cv #endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp new file mode 100644 index 00000000000..1653d27750f --- /dev/null +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -0,0 +1,55 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +using namespace cv; + +TEST( PoseGraph, sphereG2O ) +{ + //TODO: add code itself + + //DEBUG + cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_INFO); + + // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ + // Connected paper: + // L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert. + // Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization. + // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015. + + std::string filename = cvtest::TS::ptr()->get_data_path() + "sphere_bignoise_vertex3.g2o"; + //std::string filename = "C:\\Users\\rvasilik\\Downloads\\torus3D.g2o"; + PoseGraph pg(filename); + + //writePg(pg, "C:\\Temp\\g2opt\\in.obj"); + + double t0 = cv::getTickCount(); + + pg.optimize(); + + double t1 = cv::getTickCount(); + + std::cout << "time: " << (t1 - t0) / cv::getTickFrequency() << std::endl; + + //writePg(pg, "C:\\Temp\\g2opt\\out.obj"); + + viz::Viz3d debug("debug"); + std::vector sv, dv; + for (const auto& e : pg.edges) + { + size_t sid = e.sourceNodeId, tid = e.targetNodeId; + Point3d sp = pg.nodes.at(sid).getPose().translation(); + Point3d tp = pg.nodes.at(tid).getPose().translation(); + sv.push_back(sp); + dv.push_back(tp - sp); + } + debug.showWidget("after", viz::WCloudNormals(sv, dv, 1, 1, viz::Color::green())); + debug.spin(); + +} + +}} // namespace \ No newline at end of file From afd11f3f037d37714d5c6d8858c9cc8f6a22447e Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 29 Mar 2021 00:31:12 +0300 Subject: [PATCH 30/51] minor: prints --- modules/rgbd/src/pose_graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index dcb6fde5bf4..daadf5e7036 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -736,7 +736,7 @@ void PoseGraph::optimize() lambdaLevMarq *= lmUpFactor; lmUpFactor *= 2.0; - CV_LOG_INFO(NULL, "LM up: " << lambdaLevMarq << ", old energy = " << oldEnergy); + CV_LOG_INFO(NULL, "LM goes up, lambda: " << lambdaLevMarq << ", old energy: " << oldEnergy); } else { @@ -756,7 +756,7 @@ void PoseGraph::optimize() oldEnergy = energy; - CV_LOG_INFO(NULL, "LM down: " << lambdaLevMarq << " step quality: " << stepQuality); + CV_LOG_INFO(NULL, "LM goes down, lambda: " << lambdaLevMarq << " step quality: " << stepQuality); } iter++; From 6f4da67c62ca99c13f390b8c154e463f891d0204 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 29 Mar 2021 02:55:57 +0300 Subject: [PATCH 31/51] Pose Graph interface settled --- .../opencv2/rgbd/detail/pose_graph.hpp | 198 +++--------- modules/rgbd/src/pose_graph.cpp | 281 +++++++++++------- modules/rgbd/test/test_pose_graph.cpp | 129 ++++++-- 3 files changed, 317 insertions(+), 291 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp index 46495963aba..917f68f5494 100644 --- a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp +++ b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp @@ -21,169 +21,47 @@ namespace kinfu namespace detail { -// TODO: text about unstable API -class PoseGraph +// ATTENTION! This class is used internally in Large KinFu. +// It has been pushed to publicly available headers for tests only. +// Source compatibility of this API is not guaranteed in the future. + +// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems +// The pose graph format, cost function and optimization techniques +// repeat the ones used in Ceres 3D Pose Graph Optimization: +// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet +class CV_EXPORTS_W PoseGraph { public: - struct Pose3d - { - Vec3d t; - Quatd q; - - Pose3d() : t(), q(1, 0, 0, 0) { } - - Pose3d(const Matx33d& rotation, const Vec3d& translation) - : t(translation), q(Quatd::createFromRotMat(rotation).normalize()) - { } - - explicit Pose3d(const Matx44d& pose) : - Pose3d(pose.get_minor<3, 3>(0, 0), Vec3d(pose(0, 3), pose(1, 3), pose(2, 3))) - { } - - inline Pose3d operator*(const Pose3d& otherPose) const - { - Pose3d out(*this); - out.t += q.toRotMat3x3(QUAT_ASSUME_UNIT) * otherPose.t; - out.q = out.q * otherPose.q; - return out; - } - - Affine3d getAffine() const - { - return Affine3d(q.toRotMat3x3(QUAT_ASSUME_UNIT), t); - } - - inline Pose3d inverse() const - { - Pose3d out; - out.q = q.conjugate(); - out.t = -(out.q.toRotMat3x3(QUAT_ASSUME_UNIT) * t); - return out; - } - - inline void normalizeRotation() - { - q = q.normalize(); - } - }; - - /*! \class GraphNode - * \brief Defines a node/variable that is optimizable in a posegraph - * - * Detailed description - */ - struct Node - { - public: - explicit Node(size_t _nodeId, const Affine3d& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) - { } - virtual ~Node() = default; - - size_t getId() const { return nodeId; } - inline Affine3d getPose() const - { - return pose.getAffine(); - } - void setPose(const Affine3d& _pose) - { - pose = Pose3d(_pose.rotation(), _pose.translation()); - } - void setPose(const Pose3d& _pose) - { - pose = _pose; - } - void setFixed(bool val = true) { isFixed = val; } - bool isPoseFixed() const { return isFixed; } - - public: - size_t nodeId; - bool isFixed; - Pose3d pose; - }; - - /*! \class PoseGraphEdge - * \brief Defines the constraints between two PoseGraphNodes - * - * Detailed description - */ - struct Edge - { - public: - Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information = Matx66f::eye()); - - virtual ~Edge() = default; - - size_t getSourceNodeId() const { return sourceNodeId; } - size_t getTargetNodeId() const { return targetNodeId; } - - bool operator==(const Edge& edge) - { - if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || - (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) - return true; - return false; - } - - public: - size_t sourceNodeId; - size_t targetNodeId; - Pose3d pose; - Matx66f sqrtInfo; - }; - - public: - explicit PoseGraph() {}; - virtual ~PoseGraph() = default; - - //! PoseGraph can be copied/cloned - PoseGraph(const PoseGraph&) = default; - PoseGraph& operator=(const PoseGraph&) = default; - - // can be used for debugging - PoseGraph(const std::string& g2oFileName); - void readG2OFile(const std::string& g2oFileName); - void writeToObjFile(const std::string& objFname) const; - - void addNode(const Node& node) - { - size_t id = node.getId(); - const auto& it = nodes.find(id); - if (it != nodes.end()) - { - std::cout << "duplicated node, id=" << id << std::endl; - nodes.insert(it, { id, node }); - } - else - { - nodes.insert({ id, node }); - } - } - void addEdge(const Edge& edge) { edges.push_back(edge); } - - bool nodeExists(size_t nodeId) const - { - return (nodes.find(nodeId) != nodes.end()); - } - - bool isValid() const; - - size_t getNumNodes() const { return nodes.size(); } - size_t getNumEdges() const { return edges.size(); } - - public: - - void optimize(const cv::TermCriteria& tc); - - // used during optimization - // nodes is a set of parameters to be used instead of contained in the graph - double calcEnergy(const std::map& newNodes) const; - -//TODO: pImpl - std::map nodes; - std::vector edges; + CV_WRAP static Ptr create(); + virtual ~PoseGraph() = 0; + + // Node may have any id >= 0 + virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0; + virtual bool isNodeExist(size_t nodeId) const = 0; + virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0; + virtual bool isNodeFixed(size_t nodeId) = 0; + virtual Affine3d getNodePose(size_t nodeId) = 0; + virtual std::vector getNodesIds() const = 0; + virtual size_t getNumNodes() const = 0; + + // Edges have consequent indices starting from 0 + virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()) = 0; + virtual size_t getEdgeStart(size_t i) const = 0; + virtual size_t getEdgeEnd(size_t i) const = 0; + virtual size_t getNumEdges() const = 0; + + // checks if graph is connected and each edge connects exactly 2 nodes + virtual bool isValid() const = 0; + + // Termination criteria are max number of iterations and min relative energy change to current energy + // Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize + virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6)) = 0; + + // calculate cost function based on current nodes parameters + virtual double calcEnergy() const = 0; }; + } // namespace detail } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index daadf5e7036..e3511377c12 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -2,7 +2,7 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -#include "pose_graph.hpp" +#include "precomp.hpp" #include #include @@ -106,6 +106,175 @@ namespace cv namespace kinfu { +class PoseGraphImpl : public detail::PoseGraph +{ + struct Pose3d + { + Vec3d t; + Quatd q; + + Pose3d() : t(), q(1, 0, 0, 0) { } + + Pose3d(const Matx33d& rotation, const Vec3d& translation) + : t(translation), q(Quatd::createFromRotMat(rotation).normalize()) + { } + + explicit Pose3d(const Matx44d& pose) : + Pose3d(pose.get_minor<3, 3>(0, 0), Vec3d(pose(0, 3), pose(1, 3), pose(2, 3))) + { } + + inline Pose3d operator*(const Pose3d& otherPose) const + { + Pose3d out(*this); + out.t += q.toRotMat3x3(QUAT_ASSUME_UNIT) * otherPose.t; + out.q = out.q * otherPose.q; + return out; + } + + Affine3d getAffine() const + { + return Affine3d(q.toRotMat3x3(QUAT_ASSUME_UNIT), t); + } + + inline Pose3d inverse() const + { + Pose3d out; + out.q = q.conjugate(); + out.t = -(out.q.toRotMat3x3(QUAT_ASSUME_UNIT) * t); + return out; + } + + inline void normalizeRotation() + { + q = q.normalize(); + } + }; + + /*! \class GraphNode + * \brief Defines a node/variable that is optimizable in a posegraph + * + * Detailed description + */ + struct Node + { + public: + explicit Node(size_t _nodeId, const Affine3d& _pose) + : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + { } + virtual ~Node() = default; + + size_t getId() const { return nodeId; } + inline Affine3d getPose() const + { + return pose.getAffine(); + } + void setPose(const Affine3d& _pose) + { + pose = Pose3d(_pose.rotation(), _pose.translation()); + } + void setPose(const Pose3d& _pose) + { + pose = _pose; + } + void setFixed(bool val = true) { isFixed = val; } + bool isPoseFixed() const { return isFixed; } + + public: + size_t nodeId; + bool isFixed; + Pose3d pose; + }; + + /*! \class PoseGraphEdge + * \brief Defines the constraints between two PoseGraphNodes + * + * Detailed description + */ + struct Edge + { + public: + Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()); + + virtual ~Edge() = default; + + size_t getSourceNodeId() const { return sourceNodeId; } + size_t getTargetNodeId() const { return targetNodeId; } + + bool operator==(const Edge& edge) + { + if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || + (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) + return true; + return false; + } + + public: + size_t sourceNodeId; + size_t targetNodeId; + Pose3d pose; + Matx66f sqrtInfo; + }; + + +public: + virtual ~PoseGraphImpl(); + + virtual void addNode(const Node& node) CV_OVERRIDE; + virtual void addEdge(const Edge& edge) CV_OVERRIDE; + virtual bool nodeExists(size_t nodeId) CV_OVERRIDE; + + // checks if graph is connected and each edge connects exactly 2 nodes + virtual bool isValid() = 0; + + virtual Report optimize(const cv::TermCriteria& tc) = 0; + + // calculate cost function based on current nodes parameters + virtual double calcEnergy() const; + + /* + void addNode(const Node& node) + { + size_t id = node.getId(); + const auto& it = nodes.find(id); + if (it != nodes.end()) + { + std::cout << "duplicated node, id=" << id << std::endl; + nodes.insert(it, { id, node }); + } + else + { + nodes.insert({ id, node }); + } + } + void addEdge(const Edge& edge) { edges.push_back(edge); } + + bool nodeExists(size_t nodeId) const + { + return (nodes.find(nodeId) != nodes.end()); + } + + bool isValid() const; + + size_t getNumNodes() const { return nodes.size(); } + size_t getNumEdges() const { return edges.size(); } + */ + + // used during optimization + // nodes is a set of parameters to be used instead of contained in the graph + //double calcNodesEnergy(const std::map& newNodes) const; + + //TODO: pImpl + //std::map nodes; + //std::vector edges; + }; + +Ptr detail::PoseGraph::create() +{ + return makePtr(); +} + + // Cholesky decomposition of symmetrical 6x6 matrix static inline cv::Matx66d llt6(Matx66d m) { @@ -203,120 +372,10 @@ PoseGraph::PoseGraph(const std::string& g2oFileName) : readG2OFile(g2oFileName); } -static Affine3d readAffine(std::istream& input) -{ - Vec3d p; - Vec4d q; - input >> p[0] >> p[1] >> p[2]; - input >> q[1] >> q[2] >> q[3] >> q[0]; - // Normalize the quaternion to account for precision loss due to - // serialization. - return Affine3d(Quatd(q).toRotMat3x3(), p); -}; - -// Rewritten from Ceres pose graph demo: https://ceres-solver.org/ -void PoseGraph::readG2OFile(const std::string& g2oFileName) -{ - nodes.clear(); edges.clear(); - - // for debugging purposes - size_t minId = 0, maxId = 1 << 30; - - std::ifstream infile(g2oFileName.c_str()); - if (!infile) - { - CV_Error(cv::Error::StsError, "failed to open file"); - } - - while (infile.good()) - { - std::string data_type; - // Read whether the type is a node or a constraint - infile >> data_type; - if (data_type == "VERTEX_SE3:QUAT") - { - size_t id; - infile >> id; - Affine3d pose = readAffine(infile); - - if (id < minId || id >= maxId) - continue; - - Node n(id, pose); - if (id == minId) - n.setFixed(); - - // Ensure we don't have duplicate poses - const auto& it = nodes.find(id); - if (it != nodes.end()) - { - CV_LOG_INFO(NULL, "duplicated node, id=" << id); - nodes.insert(it, { id, n }); - } - else - { - nodes.insert({ id, n }); - } - } - else if (data_type == "EDGE_SE3:QUAT") - { - size_t startId, endId; - infile >> startId >> endId; - Affine3d pose = readAffine(infile); - - Matx66d info; - for (int i = 0; i < 6 && infile.good(); ++i) - { - for (int j = i; j < 6 && infile.good(); ++j) - { - infile >> info(i, j); - if (i != j) - { - info(j, i) = info(i, j); - } - } - } - - if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId)) - { - edges.push_back(Edge(startId, endId, pose, info)); - } - } - else - { - CV_Error(cv::Error::StsError, "unknown tag"); - } - - // Clear any trailing whitespace from the line - infile >> std::ws; - } -} - - -// Writes edge-only model of how nodes are located in space -void PoseGraph::writeToObjFile(const std::string& fname) const -{ - std::fstream of(fname, std::fstream::out); - for (const auto& n : nodes) - { - Point3d d = n.second.getPose().translation(); - of << "v " << d.x << " " << d.y << " " << d.z << std::endl; - } - for (const auto& e : edges) - { - size_t sid = e.sourceNodeId, tid = e.targetNodeId; - of << "l " << sid + 1 << " " << tid + 1 << std::endl; - } - of.close(); -}; - ////////////////////////// // Optimization itself // //////////////////////// - - - static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd targetQuat, Vec3d targetTrans, Quatd rotMeasured, Vec3d transMeasured, Matx66d sqrtInfoMatrix, bool needJacobians, Matx& sqj, Matx& stj, diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 1653d27750f..b7d887b53f2 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -8,11 +8,95 @@ namespace opencv_test { namespace { using namespace cv; -TEST( PoseGraph, sphereG2O ) +static Affine3d readAffine(std::istream& input) +{ + Vec3d p; + Vec4d q; + input >> p[0] >> p[1] >> p[2]; + input >> q[1] >> q[2] >> q[3] >> q[0]; + // Normalize the quaternion to account for precision loss due to + // serialization. + return Affine3d(Quatd(q).toRotMat3x3(), p); +}; + +// Rewritten from Ceres pose graph demo: https://ceres-solver.org/ +static Ptr readG2OFile(const std::string& g2oFileName) { - //TODO: add code itself + Ptr pg = kinfu::detail::PoseGraph::create(); + + // for debugging purposes + size_t minId = 0, maxId = 1 << 30; - //DEBUG + std::ifstream infile(g2oFileName.c_str()); + if (!infile) + { + CV_Error(cv::Error::StsError, "failed to open file"); + } + + while (infile.good()) + { + std::string data_type; + // Read whether the type is a node or a constraint + infile >> data_type; + if (data_type == "VERTEX_SE3:QUAT") + { + size_t id; + infile >> id; + Affine3d pose = readAffine(infile); + + if (id < minId || id >= maxId) + continue; + + bool fixed = (id == minId); + + // Ensure we don't have duplicate poses + if (pg->isNodeExist(id)) + { + CV_LOG_INFO(NULL, "duplicated node, id=" << id); + } + pg->addNode(id, pose, fixed); + } + else if (data_type == "EDGE_SE3:QUAT") + { + size_t startId, endId; + infile >> startId >> endId; + Affine3d pose = readAffine(infile); + + Matx66d info; + for (int i = 0; i < 6 && infile.good(); ++i) + { + for (int j = i; j < 6 && infile.good(); ++j) + { + infile >> info(i, j); + if (i != j) + { + info(j, i) = info(i, j); + } + } + } + + if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId)) + { + pg->addEdge(startId, endId, pose, info); + } + } + else + { + CV_Error(cv::Error::StsError, "unknown tag"); + } + + // Clear any trailing whitespace from the line + infile >> std::ws; + } +} + +// Turn it on if you want to see resulting pose graph nodes +static const bool writeToObjFile = false; + + +TEST( PoseGraph, sphereG2O ) +{ + //TODO: optional cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_INFO); // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ @@ -22,33 +106,38 @@ TEST( PoseGraph, sphereG2O ) // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015. std::string filename = cvtest::TS::ptr()->get_data_path() + "sphere_bignoise_vertex3.g2o"; - //std::string filename = "C:\\Users\\rvasilik\\Downloads\\torus3D.g2o"; - PoseGraph pg(filename); - - //writePg(pg, "C:\\Temp\\g2opt\\in.obj"); + Ptr pg = readG2OFile(filename); double t0 = cv::getTickCount(); - pg.optimize(); + int iters = pg->optimize(); + double t1 = cv::getTickCount(); std::cout << "time: " << (t1 - t0) / cv::getTickFrequency() << std::endl; - //writePg(pg, "C:\\Temp\\g2opt\\out.obj"); - - viz::Viz3d debug("debug"); - std::vector sv, dv; - for (const auto& e : pg.edges) + if (writeToObjFile) { - size_t sid = e.sourceNodeId, tid = e.targetNodeId; - Point3d sp = pg.nodes.at(sid).getPose().translation(); - Point3d tp = pg.nodes.at(tid).getPose().translation(); - sv.push_back(sp); - dv.push_back(tp - sp); + // Write edge-only model of how nodes are located in space + std::string fname = "pgout.obj"; + std::fstream of(fname, std::fstream::out); + std::vector ids = pg->getNodesIds(); + for (const size_t& id : ids) + { + Point3d d = pg->getNodePose(id).translation(); + of << "v " << d.x << " " << d.y << " " << d.z << std::endl; + } + + size_t esz = pg->getNumEdges(); + for (size_t i = 0; i < esz; i++) + { + size_t sid = pg->getEdgeStart(i), tid = pg->getEdgeEnd(i); + of << "l " << sid + 1 << " " << tid + 1 << std::endl; + } + + of.close(); } - debug.showWidget("after", viz::WCloudNormals(sv, dv, 1, 1, viz::Color::green())); - debug.spin(); } From c6f730fd71f03b06c3ce3c40e45ad856891d8f98 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:24:58 +0300 Subject: [PATCH 32/51] Pose graph interface and its use updated --- .../opencv2/rgbd/detail/pose_graph.hpp | 18 +++-------- modules/rgbd/src/large_kinfu.cpp | 10 +++++-- modules/rgbd/src/submap.hpp | 30 ++++++++----------- modules/rgbd/test/test_pose_graph.cpp | 2 ++ 4 files changed, 26 insertions(+), 34 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp index 917f68f5494..9b0372dbef3 100644 --- a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp +++ b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp @@ -1,17 +1,7 @@ #ifndef OPENCV_RGBD_GRAPH_NODE_H #define OPENCV_RGBD_GRAPH_NODE_H -#include -#include - #include "opencv2/core/affine.hpp" -#if defined(HAVE_EIGEN) -#include -#include -#include "opencv2/core/eigen.hpp" -#endif - -#include "sparse_block_matrix.hpp" #include "opencv2/core/quaternion.hpp" namespace cv @@ -32,15 +22,15 @@ namespace detail class CV_EXPORTS_W PoseGraph { public: - CV_WRAP static Ptr create(); - virtual ~PoseGraph() = 0; + static Ptr create(); + virtual ~PoseGraph(); // Node may have any id >= 0 virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0; virtual bool isNodeExist(size_t nodeId) const = 0; virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0; - virtual bool isNodeFixed(size_t nodeId) = 0; - virtual Affine3d getNodePose(size_t nodeId) = 0; + virtual bool isNodeFixed(size_t nodeId) const = 0; + virtual Affine3d getNodePose(size_t nodeId) const = 0; virtual std::vector getNodesIds() const = 0; virtual size_t getNumNodes() const = 0; diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index f02a499be95..ea41e002f60 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -278,9 +278,15 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) if(isMapUpdated) { // TODO: Convert constraints to posegraph - PoseGraph poseGraph = submapMgr->MapToPoseGraph(); std::cout << "Created posegraph\n"; - poseGraph.optimize(); + Ptr poseGraph = submapMgr->MapToPoseGraph(); + int iters = poseGraph->optimize(); + if (iters < 0) + { + CV_LOG_INFO(NULL, "Failed to perform pose graph optimization"); + return false; + } + submapMgr->PoseGraphToMap(poseGraph); } diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 23f56f7bc25..ad41c47cb34 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -13,7 +13,7 @@ #include "hash_tsdf.hpp" #include "opencv2/core/mat.inl.hpp" -#include "pose_graph.hpp" +#include "opencv2/rgbd/detail/pose_graph.hpp" namespace cv { @@ -166,15 +166,15 @@ class SubmapManager int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); bool updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); - PoseGraph MapToPoseGraph(); - void PoseGraphToMap(const PoseGraph& updatedPoseGraph); + Ptr MapToPoseGraph(); + void PoseGraphToMap(const Ptr& updatedPoseGraph); VolumeParams volumeParams; std::vector> submapList; IdToActiveSubmaps activeSubmaps; - PoseGraph poseGraph; + Ptr poseGraph; }; template @@ -494,9 +494,9 @@ bool SubmapManager::updateMap(int _frameId, std::vector _frame } template -PoseGraph SubmapManager::MapToPoseGraph() +Ptr SubmapManager::MapToPoseGraph() { - PoseGraph localPoseGraph; + Ptr localPoseGraph = detail::PoseGraph::create(); for(const auto& currSubmap : submapList) { @@ -506,32 +506,26 @@ PoseGraph SubmapManager::MapToPoseGraph() // TODO: Handle case with duplicate constraints A -> B and B -> A /* Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); */ Matx66f informationMatrix = Matx66f::eye(); - PoseGraph::Edge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); - localPoseGraph.addEdge(currEdge); + localPoseGraph->addEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); } } for(const auto& currSubmap : submapList) { - PoseGraph::Node currNode(currSubmap->id, currSubmap->pose); - if(currSubmap->id == 0) - { - currNode.setFixed(); - } - localPoseGraph.addNode(currNode); + localPoseGraph->addNode(currSubmap->id, currSubmap->pose, (currSubmap->id == 0)); } return localPoseGraph; } template -void SubmapManager::PoseGraphToMap(const PoseGraph &updatedPoseGraph) +void SubmapManager::PoseGraphToMap(const Ptr& updatedPoseGraph) { for(const auto& currSubmap : submapList) { - const PoseGraph::Node& currNode = updatedPoseGraph.nodes.at(currSubmap->id); - if(!currNode.isPoseFixed()) - currSubmap->pose = currNode.getPose(); + Affine3d pose = updatedPoseGraph->getNodePose(currSubmap->id); + if(!updatedPoseGraph->isNodeFixed(currSubmap->id)) + currSubmap->pose = pose; std::cout << "Current node: " << currSubmap->id << " Updated Pose: \n" << currSubmap->pose.matrix << std::endl; } } diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index b7d887b53f2..cb94465767a 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -88,6 +88,8 @@ static Ptr readG2OFile(const std::string& g2oFileName) // Clear any trailing whitespace from the line infile >> std::ws; } + + return pg; } // Turn it on if you want to see resulting pose graph nodes From 9969c10f51f4b2c177708c7fc0570964604ed44d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:25:31 +0300 Subject: [PATCH 33/51] cos -> std::cos --- modules/rgbd/src/fast_icp.cpp | 4 ++-- modules/rgbd/src/nonrigid_icp.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index ab25eb94f0d..07eb84d19fd 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -504,7 +504,7 @@ void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts const Normals np(newPts), nn(newNrm); GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose, intrinsics.scale(level).makeProjector(), - distanceThreshold*distanceThreshold, cos(angleThreshold)); + distanceThreshold*distanceThreshold, std::cos(angleThreshold)); Range range(0, newPts.rows); const int nstripes = -1; parallel_for_(range, invoker, nstripes); @@ -590,7 +590,7 @@ void ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& ne sizeof(pose.matrix.val)), fxy.val, cxy.val, distanceThreshold*distanceThreshold, - cos(angleThreshold), + std::cos(angleThreshold), ocl::KernelArg::Local(lsz), ocl::KernelArg::WriteOnlyNoSize(groupedSumGpu) ); diff --git a/modules/rgbd/src/nonrigid_icp.cpp b/modules/rgbd/src/nonrigid_icp.cpp index f2bbc5ffb2a..9bac595a4c2 100644 --- a/modules/rgbd/src/nonrigid_icp.cpp +++ b/modules/rgbd/src/nonrigid_icp.cpp @@ -350,7 +350,7 @@ bool ICPImpl::estimateWarpNodes(WarpField& currentWarp, const Affine3f &pose, Vec3f diff = oldPoints.at(y, x) - Vec3f(newP); if(diff.dot(diff) > 0.0004f) continue; - if(abs(newN.dot(oldNormals.at(y, x))) < cos((float)CV_PI / 2)) continue; + if(abs(newN.dot(oldNormals.at(y, x))) < std::cos((float)CV_PI / 2)) continue; float rd = newN.dot(diff); From 38a9b29deb3b2a774033415fae0d5d5efc9b2355 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:26:01 +0300 Subject: [PATCH 34/51] cout -> CV_LOG_INFO --- modules/rgbd/src/large_kinfu.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index ea41e002f60..8babd45fd60 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -225,14 +225,14 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - std::cout << "Current frameID: " << frameCounter << "\n"; + CV_LOG_INFO(NULL, "Current frameID: " << frameCounter); for (const auto& it : submapMgr->activeSubmaps) { int currTrackingId = it.first; auto submapData = it.second; Ptr> currTrackingSubmap = submapMgr->getSubmap(currTrackingId); Affine3f affine; - std::cout << "Current tracking ID: " << currTrackingId << std::endl; + CV_LOG_INFO(NULL, "Current tracking ID: " << currTrackingId); if(frameCounter == 0) //! Only one current tracking map { @@ -249,7 +249,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) currTrackingSubmap->composeCameraPose(affine); else { - std::cout << "Tracking failed" << std::endl; + CV_LOG_INFO(NULL, "Tracking failed"); continue; } @@ -268,8 +268,8 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) currTrackingSubmap->updatePyrPointsNormals(params.pyramidLevels); - std::cout << "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n"; - std::cout << "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n"; + CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks()); + CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter)); } //4. Update map @@ -278,8 +278,8 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) if(isMapUpdated) { // TODO: Convert constraints to posegraph - std::cout << "Created posegraph\n"; Ptr poseGraph = submapMgr->MapToPoseGraph(); + CV_LOG_INFO(NULL, "Created posegraph"); int iters = poseGraph->optimize(); if (iters < 0) { @@ -290,7 +290,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) submapMgr->PoseGraphToMap(poseGraph); } - std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; + CV_LOG_INFO(NULL, "Number of submaps: " << submapMgr->submapList.size()); frameCounter++; return true; From 0be999dd7359c4c5350f3788198ab96a4e2e820c Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:28:43 +0300 Subject: [PATCH 35/51] pose graph interface updated: implementation --- modules/rgbd/src/pose_graph.cpp | 155 +++++++++++++++++++++++--------- 1 file changed, 113 insertions(+), 42 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index e3511377c12..11ec640525b 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -8,6 +8,7 @@ #include #include #include +#include "sparse_block_matrix.hpp" // matrix form of conjugation static const cv::Matx44d M_Conj{ 1, 0, 0, 0, @@ -105,6 +106,8 @@ namespace cv { namespace kinfu { +namespace detail +{ class PoseGraphImpl : public detail::PoseGraph { @@ -218,60 +221,120 @@ class PoseGraphImpl : public detail::PoseGraph public: - virtual ~PoseGraphImpl(); - - virtual void addNode(const Node& node) CV_OVERRIDE; - virtual void addEdge(const Edge& edge) CV_OVERRIDE; - virtual bool nodeExists(size_t nodeId) CV_OVERRIDE; + PoseGraphImpl() : nodes(), edges() + { } + virtual ~PoseGraphImpl() CV_OVERRIDE + { } + + // Node may have any id >= 0 + virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) CV_OVERRIDE; + virtual bool isNodeExist(size_t nodeId) const CV_OVERRIDE + { + return (nodes.find(nodeId) != nodes.end()); + } - // checks if graph is connected and each edge connects exactly 2 nodes - virtual bool isValid() = 0; + virtual bool setNodeFixed(size_t nodeId, bool fixed) CV_OVERRIDE + { + auto it = nodes.find(nodeId); + if (it != nodes.end()) + { + it->second.isFixed = fixed; + return true; + } + else + return false; + } - virtual Report optimize(const cv::TermCriteria& tc) = 0; + virtual bool isNodeFixed(size_t nodeId) const CV_OVERRIDE + { + auto it = nodes.find(nodeId); + if (it != nodes.end()) + return it->second.isFixed; + else + return false; + } - // calculate cost function based on current nodes parameters - virtual double calcEnergy() const; + virtual Affine3d getNodePose(size_t nodeId) const CV_OVERRIDE + { + auto it = nodes.find(nodeId); + if (it != nodes.end()) + return it->second.getPose(); + else + return Affine3d(); + } - /* - void addNode(const Node& node) + virtual std::vector getNodesIds() const CV_OVERRIDE + { + std::vector ids; + for (const auto& it : nodes) { - size_t id = node.getId(); - const auto& it = nodes.find(id); - if (it != nodes.end()) - { - std::cout << "duplicated node, id=" << id << std::endl; - nodes.insert(it, { id, node }); - } - else - { - nodes.insert({ id, node }); - } + ids.push_back(it.first); } - void addEdge(const Edge& edge) { edges.push_back(edge); } + return ids; + } - bool nodeExists(size_t nodeId) const - { - return (nodes.find(nodeId) != nodes.end()); - } + virtual size_t getNumNodes() const CV_OVERRIDE + { + return nodes.size(); + } + + // Edges have consequent indices starting from 0 + virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()) CV_OVERRIDE + { + Edge e(_sourceNodeId, _targetNodeId, _transformation, _information); + edges.push_back(e); + } - bool isValid() const; + virtual size_t getEdgeStart(size_t i) const CV_OVERRIDE + { + return edges[i].sourceNodeId; + } - size_t getNumNodes() const { return nodes.size(); } - size_t getNumEdges() const { return edges.size(); } - */ + virtual size_t getEdgeEnd(size_t i) const CV_OVERRIDE + { + return edges[i].targetNodeId; + } - // used during optimization - // nodes is a set of parameters to be used instead of contained in the graph - //double calcNodesEnergy(const std::map& newNodes) const; + virtual size_t getNumEdges() const CV_OVERRIDE + { + return edges.size(); + } - //TODO: pImpl - //std::map nodes; - //std::vector edges; - }; + // checks if graph is connected and each edge connects exactly 2 nodes + virtual bool isValid() const CV_OVERRIDE; -Ptr detail::PoseGraph::create() + // calculate cost function based on current nodes parameters + virtual double calcEnergy() const CV_OVERRIDE; + + // calculate cost function based on provided nodes parameters + double calcEnergyNodes(const std::map& newNodes) const; + + // Termination criteria are max number of iterations and min relative energy change to current energy + // Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize + virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-6)) CV_OVERRIDE; + + std::map nodes; + std::vector edges; +}; + + +void PoseGraphImpl::addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) { - return makePtr(); + Node node(_nodeId, _pose); + node.isFixed = fixed; + + size_t id = node.id; + const auto& it = nodes.find(id); + if (it != nodes.end()) + { + std::cout << "duplicated node, id=" << id << std::endl; + nodes.insert(it, { id, node }); + } + else + { + nodes.insert({ id, node }); + } } @@ -839,12 +902,20 @@ void PoseGraph::optimize() CV_LOG_INFO(NULL, "Finish reason: max number of iterations reached"); } #else -void PoseGraph::optimize() +int PoseGraphImpl::optimize(const cv::TermCriteria& /*tc*/) { CV_Error(Error::StsNotImplemented, "Eigen library required for sparse matrix solve during pose graph optimization, dense solver is not implemented"); } #endif +Ptr detail::PoseGraph::create() +{ + return makePtr(); +} + +PoseGraph::~PoseGraph() { } + +} // namespace detail } // namespace kinfu } // namespace cv From de7a39c9231e4422c666ab871afe731d68c65c05 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:29:29 +0300 Subject: [PATCH 36/51] Pose Graph Node and Edge: extra fields dropped --- modules/rgbd/src/pose_graph.cpp | 39 +++++++++++---------------------- 1 file changed, 13 insertions(+), 26 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 11ec640525b..8f006436d70 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -162,12 +162,10 @@ class PoseGraphImpl : public detail::PoseGraph { public: explicit Node(size_t _nodeId, const Affine3d& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + : id(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) { } - virtual ~Node() = default; - size_t getId() const { return nodeId; } - inline Affine3d getPose() const + Affine3d getPose() const { return pose.getAffine(); } @@ -175,15 +173,9 @@ class PoseGraphImpl : public detail::PoseGraph { pose = Pose3d(_pose.rotation(), _pose.translation()); } - void setPose(const Pose3d& _pose) - { - pose = _pose; - } - void setFixed(bool val = true) { isFixed = val; } - bool isPoseFixed() const { return isFixed; } public: - size_t nodeId; + size_t id; bool isFixed; Pose3d pose; }; @@ -196,18 +188,13 @@ class PoseGraphImpl : public detail::PoseGraph struct Edge { public: - Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information = Matx66f::eye()); - - virtual ~Edge() = default; - - size_t getSourceNodeId() const { return sourceNodeId; } - size_t getTargetNodeId() const { return targetNodeId; } + explicit Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()); bool operator==(const Edge& edge) { - if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || - (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) + if ((edge.sourceNodeId == sourceNodeId && edge.targetNodeId == targetNodeId) || + (edge.sourceNodeId == targetNodeId && edge.targetNodeId == sourceNodeId)) return true; return false; } @@ -359,12 +346,12 @@ static inline cv::Matx66d llt6(Matx66d m) return L; } -PoseGraph::Edge::Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, - const Matx66f& _information) : - sourceNodeId(_sourceNodeId), - targetNodeId(_targetNodeId), - pose(_transformation.rotation(), _transformation.translation()), - sqrtInfo(llt6(_information)) +PoseGraphImpl::Edge::Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information) : + sourceNodeId(_sourceNodeId), + targetNodeId(_targetNodeId), + pose(_transformation.rotation(), _transformation.translation()), + sqrtInfo(llt6(_information)) { } bool PoseGraph::isValid() const From c8d3c6efce986a9799e6b640e34ec30a927cafa0 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:31:04 +0300 Subject: [PATCH 37/51] more minor refactor-like fixes --- modules/rgbd/src/pose_graph.cpp | 50 ++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 8f006436d70..eeff046f4e8 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -354,7 +354,8 @@ PoseGraphImpl::Edge::Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affi sqrtInfo(llt6(_information)) { } -bool PoseGraph::isValid() const + +bool PoseGraphImpl::isValid() const { size_t numNodes = getNumNodes(); size_t numEdges = getNumEdges(); @@ -379,13 +380,13 @@ bool PoseGraph::isValid() const const Edge& potentialEdge = edges.at(i); size_t nextNodeId = (size_t)(-1); - if (potentialEdge.getSourceNodeId() == currNodeId) + if (potentialEdge.sourceNodeId == currNodeId) { - nextNodeId = potentialEdge.getTargetNodeId(); + nextNodeId = potentialEdge.targetNodeId; } - else if (potentialEdge.getTargetNodeId() == currNodeId) + else if (potentialEdge.targetNodeId == currNodeId) { - nextNodeId = potentialEdge.getSourceNodeId(); + nextNodeId = potentialEdge.sourceNodeId; } if (nextNodeId != (size_t)(-1)) { @@ -406,8 +407,8 @@ bool PoseGraph::isValid() const { const Edge& edge = edges.at(i); // edges have spurious source/target nodes - if ((nodesVisited.count(edge.getSourceNodeId()) != 1) || - (nodesVisited.count(edge.getTargetNodeId()) != 1)) + if ((nodesVisited.count(edge.sourceNodeId) != 1) || + (nodesVisited.count(edge.targetNodeId) != 1)) { invalidEdgeNode = true; break; @@ -416,11 +417,6 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -PoseGraph::PoseGraph(const std::string& g2oFileName) : - nodes(), edges() -{ - readG2OFile(g2oFileName); -} ////////////////////////// // Optimization itself // @@ -498,14 +494,20 @@ static inline double poseError(Quatd sourceQuat, Vec3d sourceTrans, Quatd target } +double PoseGraphImpl::calcEnergy() const +{ + return calcEnergyNodes(nodes); +} + + // estimate current energy -double PoseGraph::calcEnergy(const std::map& newNodes) const +double PoseGraphImpl::calcEnergyNodes(const std::map& newNodes) const { double totalErr = 0; for (const auto& e : edges) { - Pose3d srcP = newNodes.at(e.getSourceNodeId()).pose; - Pose3d tgtP = newNodes.at(e.getTargetNodeId()).pose; + Pose3d srcP = newNodes.at(e.sourceNodeId).pose; + Pose3d tgtP = newNodes.at(e.targetNodeId).pose; Vec6d res; Matx stj, ttj; @@ -592,7 +594,7 @@ void PoseGraph::optimize() std::map idToPlace; for (const auto& ni : nodes) { - if (!ni.second.isPoseFixed()) + if (!ni.second.isFixed) { idToPlace[ni.first] = placesIds.size(); placesIds.push_back(ni.first); @@ -618,17 +620,19 @@ void PoseGraph::optimize() BlockSparseMat jtj(nVarNodes); std::vector jtb(nVars); - double energy = calcEnergy(nodes); + double energy = calcEnergyNodes(nodes); double oldEnergy = energy; CV_LOG_INFO(NULL, "#s" << " energy: " << energy); // options // stop conditions - const unsigned int maxIterations = 100; + bool checkIterations = (tc.type & TermCriteria::COUNT); + bool checkEps = (tc.type & TermCriteria::EPS); + const unsigned int maxIterations = tc.maxCount; const double minGradientTolerance = 1e-6; const double stepNorm2Tolerance = 1e-6; - const double relEnergyDeltaTolerance = 1e-6; + const double relEnergyDeltaTolerance = tc.epsilon; // normalize jacobian columns for better conditioning // slows down sparse solver, but maybe this'd be useful for some other solver const bool jacobiScaling = false; @@ -674,14 +678,14 @@ void PoseGraph::optimize() // fill jtj and jtb for (const auto& e : edges) { - size_t srcId = e.getSourceNodeId(), dstId = e.getTargetNodeId(); + size_t srcId = e.sourceNodeId, dstId = e.targetNodeId; const Node& srcNode = nodes.at(srcId); const Node& dstNode = nodes.at(dstId); Pose3d srcP = srcNode.pose; Pose3d tgtP = dstNode.pose; - bool srcFixed = srcNode.isPoseFixed(); - bool dstFixed = dstNode.isPoseFixed(); + bool srcFixed = srcNode.isFixed; + bool dstFixed = dstNode.isFixed; Vec6d res; Matx stj, ttj; @@ -823,7 +827,7 @@ void PoseGraph::optimize() } // calc energy with temp nodes - energy = calcEnergy(tempNodes); + energy = calcEnergyNodes(tempNodes); costChange = oldEnergy - energy; From d31536feff8b589fdf3f28cf41fa11b54a07e87e Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:31:48 +0300 Subject: [PATCH 38/51] return and finish condition fixed --- modules/rgbd/src/pose_graph.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index eeff046f4e8..065c6835939 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -577,13 +577,12 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto } -void PoseGraph::optimize() +int PoseGraphImpl::optimize(const cv::TermCriteria& tc) { if (!isValid()) { - CV_Error(Error::StsBadArg, - "Invalid PoseGraph that is either not connected or has invalid nodes"); - return; + CV_LOG_INFO(NULL, "Invalid PoseGraph that is either not connected or has invalid nodes"); + return -1; } size_t numNodes = getNumNodes(); @@ -605,13 +604,13 @@ void PoseGraph::optimize() if (!nVarNodes) { CV_LOG_INFO(NULL, "PoseGraph contains no non-constant nodes, skipping optimization"); - return; + return -1; } if (numEdges == 0) { CV_LOG_INFO(NULL, "PoseGraph has no edges, no optimization to be done"); - return; + return -1; } CV_LOG_INFO(NULL, "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges"); @@ -876,11 +875,12 @@ void PoseGraph::optimize() tooLong = (iter >= maxIterations); - done = (tooLong || smallGradient || smallStep || smallEnergyDelta); + done = ( (checkIterations && tooLong) || smallGradient || smallStep || (checkEps && smallEnergyDelta) ); } } - bool found = (smallGradient || smallStep || smallEnergyDelta); + // if maxIterations is given as a stop criteria, let's consider tooLong as a successful finish + bool found = (smallGradient || smallStep || smallEnergyDelta || (checkIterations && tooLong)); CV_LOG_INFO(NULL, "Finished: " << (found ? "" : "not") << "found"); if (smallGradient) @@ -891,7 +891,10 @@ void PoseGraph::optimize() CV_LOG_INFO(NULL, "Finish reason: relative energy change between iterations dropped below threshold"); if (tooLong) CV_LOG_INFO(NULL, "Finish reason: max number of iterations reached"); + + return (found ? iter : -1); } + #else int PoseGraphImpl::optimize(const cv::TermCriteria& /*tc*/) { From a3d67410de62fd60b0d39ca83453305666f85479 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:32:53 +0300 Subject: [PATCH 39/51] more updates to test --- modules/rgbd/test/test_pose_graph.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index cb94465767a..e6ba588e30d 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -94,30 +94,33 @@ static Ptr readG2OFile(const std::string& g2oFileName) // Turn it on if you want to see resulting pose graph nodes static const bool writeToObjFile = false; - +// Turn if off if you don't need log messages +static const bool verbose = true; TEST( PoseGraph, sphereG2O ) { - //TODO: optional - cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_INFO); - // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ // Connected paper: // L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert. // Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization. // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015. - std::string filename = cvtest::TS::ptr()->get_data_path() + "sphere_bignoise_vertex3.g2o"; + std::string filename = cvtest::TS::ptr()->get_data_path() + "rgbd/sphere_bignoise_vertex3.g2o"; Ptr pg = readG2OFile(filename); - double t0 = cv::getTickCount(); + if (verbose) + { + cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_INFO); + } int iters = pg->optimize(); + ASSERT_GE(iters, 0); + ASSERT_LE(iters, 36); // should converge in 36 iterations - double t1 = cv::getTickCount(); + double energy = pg->calcEnergy(); - std::cout << "time: " << (t1 - t0) / cv::getTickFrequency() << std::endl; + ASSERT_LE(energy, 1.47723e+06); // should converge to 1.47722e+06 or less if (writeToObjFile) { @@ -140,7 +143,6 @@ TEST( PoseGraph, sphereG2O ) of.close(); } - } }} // namespace \ No newline at end of file From f53b44ce098226a67284d122693481ad88ab9730 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:33:29 +0300 Subject: [PATCH 40/51] test disabled for Debug builds because 400 sec is too much --- modules/rgbd/test/test_pose_graph.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index e6ba588e30d..55ec301f88e 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -97,6 +97,7 @@ static const bool writeToObjFile = false; // Turn if off if you don't need log messages static const bool verbose = true; +#if !defined _DEBUG TEST( PoseGraph, sphereG2O ) { // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ @@ -144,5 +145,13 @@ TEST( PoseGraph, sphereG2O ) of.close(); } } +#else +TEST(PoseGraph, DISABLED) +{ + // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release + CV_UNUSED(readG2OFile); +} +#endif + }} // namespace \ No newline at end of file From 5c6c1265943dc268e4e1f7bb6364c10b107313ab Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 01:42:08 +0300 Subject: [PATCH 41/51] whitespace --- modules/rgbd/test/test_pose_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 55ec301f88e..40a766a4c42 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -141,7 +141,7 @@ TEST( PoseGraph, sphereG2O ) size_t sid = pg->getEdgeStart(i), tid = pg->getEdgeEnd(i); of << "l " << sid + 1 << " " << tid + 1 << std::endl; } - + of.close(); } } From f9adf7751e3b31194ae1a6cc69aa098d9c73614a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 16:37:19 +0300 Subject: [PATCH 42/51] Disable pose graph test if there's no Eigen --- modules/rgbd/test/test_pose_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 40a766a4c42..e580406e073 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -97,7 +97,7 @@ static const bool writeToObjFile = false; // Turn if off if you don't need log messages static const bool verbose = true; -#if !defined _DEBUG +#if !defined(_DEBUG) && defined(HAVE_EIGEN) TEST( PoseGraph, sphereG2O ) { // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ From 1e67e3c4630deb959ebf9ce807c7809e0aa7d654 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 19:26:44 +0300 Subject: [PATCH 43/51] more unused vars --- modules/rgbd/test/test_pose_graph.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index e580406e073..8e8f1899824 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -150,6 +150,8 @@ TEST(PoseGraph, DISABLED) { // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release CV_UNUSED(readG2OFile); + CV_UNUSED(writeToObjFile); + CV_UNUSED(verbose); } #endif From 5527997db2cf14129d2117b4dcdf9ad43d2dff19 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 30 Mar 2021 23:31:40 +0300 Subject: [PATCH 44/51] fixing unused function warning --- modules/rgbd/test/test_pose_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 8e8f1899824..07958f2d563 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -149,7 +149,7 @@ TEST( PoseGraph, sphereG2O ) TEST(PoseGraph, DISABLED) { // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release - CV_UNUSED(readG2OFile); + (void)(&readG2OFile); CV_UNUSED(writeToObjFile); CV_UNUSED(verbose); } From 2756e6281dff5f8fb891b2bd4b56c75e40ef63f5 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 00:31:34 +0300 Subject: [PATCH 45/51] less includes --- modules/rgbd/src/pose_graph.cpp | 4 ---- modules/rgbd/src/precomp.hpp | 2 ++ 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 065c6835939..78a331a2d94 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -4,10 +4,6 @@ #include "precomp.hpp" -#include -#include -#include -#include #include "sparse_block_matrix.hpp" // matrix form of conjugation diff --git a/modules/rgbd/src/precomp.hpp b/modules/rgbd/src/precomp.hpp index bad630705d7..4852274f3b0 100644 --- a/modules/rgbd/src/precomp.hpp +++ b/modules/rgbd/src/precomp.hpp @@ -11,8 +11,10 @@ #define __OPENCV_PRECOMP_H__ #include +#include #include #include +#include #include #include "opencv2/core/utility.hpp" From 5d13e84aaa42a03aa1a590d31d8425429df809ca Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 23:03:10 +0300 Subject: [PATCH 46/51] "verbose" removed --- modules/rgbd/test/test_pose_graph.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 07958f2d563..f1de89ae9a6 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -94,8 +94,6 @@ static Ptr readG2OFile(const std::string& g2oFileName) // Turn it on if you want to see resulting pose graph nodes static const bool writeToObjFile = false; -// Turn if off if you don't need log messages -static const bool verbose = true; #if !defined(_DEBUG) && defined(HAVE_EIGEN) TEST( PoseGraph, sphereG2O ) @@ -109,10 +107,8 @@ TEST( PoseGraph, sphereG2O ) std::string filename = cvtest::TS::ptr()->get_data_path() + "rgbd/sphere_bignoise_vertex3.g2o"; Ptr pg = readG2OFile(filename); - if (verbose) - { - cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_INFO); - } + // You may change logging level to view detailed optimization report + // For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO int iters = pg->optimize(); @@ -151,7 +147,6 @@ TEST(PoseGraph, DISABLED) // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release (void)(&readG2OFile); CV_UNUSED(writeToObjFile); - CV_UNUSED(verbose); } #endif From 8fcf78b7a3ff0df3c49382f524ea09d327f2f8f5 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 23:12:55 +0300 Subject: [PATCH 47/51] write obj to file only when debug level is raised --- modules/rgbd/test/test_pose_graph.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index f1de89ae9a6..4cb9f431305 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -92,9 +92,6 @@ static Ptr readG2OFile(const std::string& g2oFileName) return pg; } -// Turn it on if you want to see resulting pose graph nodes -static const bool writeToObjFile = false; - #if !defined(_DEBUG) && defined(HAVE_EIGEN) TEST( PoseGraph, sphereG2O ) { @@ -119,7 +116,8 @@ TEST( PoseGraph, sphereG2O ) ASSERT_LE(energy, 1.47723e+06); // should converge to 1.47722e+06 or less - if (writeToObjFile) + // Add the "--test_debug" to arguments to see resulting pose graph nodes positions + if (cvtest::debugLevel > 0) { // Write edge-only model of how nodes are located in space std::string fname = "pgout.obj"; @@ -146,7 +144,6 @@ TEST(PoseGraph, DISABLED) { // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release (void)(&readG2OFile); - CV_UNUSED(writeToObjFile); } #endif From a99db27b7f7021806723c274e4da3d169898da58 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 23:14:39 +0300 Subject: [PATCH 48/51] License + include guard --- .../rgbd/include/opencv2/rgbd/detail/pose_graph.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp index 9b0372dbef3..a611ffa9b81 100644 --- a/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp +++ b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp @@ -1,5 +1,9 @@ -#ifndef OPENCV_RGBD_GRAPH_NODE_H -#define OPENCV_RGBD_GRAPH_NODE_H +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#ifndef OPENCV_RGBD_POSE_GRAPH_HPP +#define OPENCV_RGBD_POSE_GRAPH_HPP #include "opencv2/core/affine.hpp" #include "opencv2/core/quaternion.hpp" @@ -55,4 +59,4 @@ class CV_EXPORTS_W PoseGraph } // namespace detail } // namespace kinfu } // namespace cv -#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ +#endif /* ifndef OPENCV_RGBD_POSE_GRAPH_HPP */ From c30d23cb15aea500a4d623182266ad6cc7fa76f3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 23:24:32 +0300 Subject: [PATCH 49/51] skip test using tags and SkipTestException --- modules/rgbd/test/test_pose_graph.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 4cb9f431305..227c514d176 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -92,9 +92,13 @@ static Ptr readG2OFile(const std::string& g2oFileName) return pg; } -#if !defined(_DEBUG) && defined(HAVE_EIGEN) + TEST( PoseGraph, sphereG2O ) { +#ifdef HAVE_EIGEN + // Test takes 15+ sec in Release mode and 400+ sec in Debug mode + applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG); + // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/ // Connected paper: // L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert. @@ -138,14 +142,10 @@ TEST( PoseGraph, sphereG2O ) of.close(); } -} #else -TEST(PoseGraph, DISABLED) -{ - // Disabled for Debug mode, it takes 400 sec to run test vs 15 sec in Release - (void)(&readG2OFile); -} + throw SkipTestException("Build with Eigen required for pose graph optimization"); #endif +} }} // namespace \ No newline at end of file From ebed66babcd858112926af6033fe0b77e614cb56 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 31 Mar 2021 23:49:40 +0300 Subject: [PATCH 50/51] suppress "unused function" warning --- modules/rgbd/test/test_pose_graph.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 227c514d176..9a1fb9d912a 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -143,6 +143,9 @@ TEST( PoseGraph, sphereG2O ) of.close(); } #else + // suppress "unused function" warning + (void)(&readG2OFile); + throw SkipTestException("Build with Eigen required for pose graph optimization"); #endif } From e1e5ff7daeed52642cf4eebfaf8b15ec445c9dcd Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 1 Apr 2021 14:59:50 +0300 Subject: [PATCH 51/51] minor --- modules/rgbd/test/test_pose_graph.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/modules/rgbd/test/test_pose_graph.cpp b/modules/rgbd/test/test_pose_graph.cpp index 9a1fb9d912a..a675e343c27 100644 --- a/modules/rgbd/test/test_pose_graph.cpp +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -95,7 +95,6 @@ static Ptr readG2OFile(const std::string& g2oFileName) TEST( PoseGraph, sphereG2O ) { -#ifdef HAVE_EIGEN // Test takes 15+ sec in Release mode and 400+ sec in Debug mode applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG); @@ -108,6 +107,7 @@ TEST( PoseGraph, sphereG2O ) std::string filename = cvtest::TS::ptr()->get_data_path() + "rgbd/sphere_bignoise_vertex3.g2o"; Ptr pg = readG2OFile(filename); +#ifdef HAVE_EIGEN // You may change logging level to view detailed optimization report // For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO @@ -143,9 +143,6 @@ TEST( PoseGraph, sphereG2O ) of.close(); } #else - // suppress "unused function" warning - (void)(&readG2OFile); - throw SkipTestException("Build with Eigen required for pose graph optimization"); #endif }