diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 143cdf913af..b0a7c73294e 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,15 +1,7 @@ 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") +if(NOT HAVE_EIGEN) + message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization") endif() 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/include/opencv2/rgbd/detail/pose_graph.hpp b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp new file mode 100644 index 00000000000..a611ffa9b81 --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp @@ -0,0 +1,62 @@ +// 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" + +namespace cv +{ +namespace kinfu +{ +namespace detail +{ + +// 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: + 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) const = 0; + virtual Affine3d getNodePose(size_t nodeId) const = 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 +#endif /* ifndef OPENCV_RGBD_POSE_GRAPH_HPP */ 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/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 16006713c53..8babd45fd60 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) { @@ -224,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 { @@ -248,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; } @@ -267,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 @@ -277,13 +278,19 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) if(isMapUpdated) { // TODO: Convert constraints to posegraph - PoseGraph poseGraph = submapMgr->MapToPoseGraph(); - std::cout << "Created posegraph\n"; - Optimizer::optimize(poseGraph); + Ptr poseGraph = submapMgr->MapToPoseGraph(); + CV_LOG_INFO(NULL, "Created posegraph"); + int iters = poseGraph->optimize(); + if (iters < 0) + { + CV_LOG_INFO(NULL, "Failed to perform pose graph optimization"); + return false; + } + 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; 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); diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index b525e455129..78a331a2d94 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -2,59 +2,390 @@ // 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 -#include -#include +#include "sparse_block_matrix.hpp" -#if defined(CERES_FOUND) -#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 }; +} + +// 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; + return cv::Matx43d(-x, -y, -z, + w, z, -y, + -z, w, x, + y, -x, w); +} #endif +// 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 { -bool PoseGraph::isValid() const +namespace detail +{ + +class PoseGraphImpl : public detail::PoseGraph { - int numNodes = getNumNodes(); - int numEdges = getNumEdges(); + 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; + } - if (numNodes <= 0 || numEdges <= 0) + 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) + : id(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + { } + + Affine3d getPose() const + { + return pose.getAffine(); + } + void setPose(const Affine3d& _pose) + { + pose = Pose3d(_pose.rotation(), _pose.translation()); + } + + public: + size_t id; + bool isFixed; + Pose3d pose; + }; + + /*! \class PoseGraphEdge + * \brief Defines the constraints between two PoseGraphNodes + * + * Detailed description + */ + struct Edge + { + public: + explicit Edge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()); + + bool operator==(const Edge& edge) + { + if ((edge.sourceNodeId == sourceNodeId && edge.targetNodeId == targetNodeId) || + (edge.sourceNodeId == targetNodeId && edge.targetNodeId == sourceNodeId)) + return true; + return false; + } + + public: + size_t sourceNodeId; + size_t targetNodeId; + Pose3d pose; + Matx66f sqrtInfo; + }; + + +public: + 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()); + } + + 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 bool isNodeFixed(size_t nodeId) const CV_OVERRIDE + { + auto it = nodes.find(nodeId); + if (it != nodes.end()) + return it->second.isFixed; + else + return false; + } + + 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(); + } + + virtual std::vector getNodesIds() const CV_OVERRIDE + { + std::vector ids; + for (const auto& it : nodes) + { + ids.push_back(it.first); + } + return ids; + } + + 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); + } + + virtual size_t getEdgeStart(size_t i) const CV_OVERRIDE + { + return edges[i].sourceNodeId; + } + + virtual size_t getEdgeEnd(size_t i) const CV_OVERRIDE + { + return edges[i].targetNodeId; + } + + virtual size_t getNumEdges() const CV_OVERRIDE + { + return edges.size(); + } + + // checks if graph is connected and each edge connects exactly 2 nodes + virtual bool isValid() const CV_OVERRIDE; + + // 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) +{ + 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 }); + } +} + + +// 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; +} + +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 PoseGraphImpl::isValid() const +{ + size_t numNodes = getNumNodes(); + size_t numEdges = getNumEdges(); + + if (!numNodes || !numEdges) return false; - std::unordered_set nodesVisited; - std::vector nodesToVisit; + 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(); + size_t 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++) + for (size_t i = 0; i < numEdges; i++) { - const PoseGraphEdge& potentialEdge = edges.at(i); - int nextNodeId = -1; + 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 != -1) + if (nextNodeId != (size_t)(-1)) { - std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) - << std::endl; if (nodesVisited.count(nextNodeId) == 0) { nodesToVisit.push_back(nextNodeId); @@ -63,16 +394,17 @@ bool PoseGraph::isValid() const } } - isGraphConnected = (int(nodesVisited.size()) == numNodes); - std::cout << "nodesVisited: " << nodesVisited.size() - << " IsGraphConnected: " << isGraphConnected << std::endl; + isGraphConnected = (nodesVisited.size() == numNodes); + + CV_LOG_INFO(NULL, "nodesVisited: " << nodesVisited.size() << " IsGraphConnected: " << isGraphConnected); + bool invalidEdgeNode = false; - for (int i = 0; i < numEdges; i++) + for (size_t 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)) + if ((nodesVisited.count(edge.sourceNodeId) != 1) || + (nodesVisited.count(edge.targetNodeId) != 1)) { invalidEdgeNode = true; break; @@ -81,89 +413,499 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -#if defined(CERES_FOUND) && defined(HAVE_EIGEN) -void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) + +////////////////////////// +// 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, + Matx& tqj, Matx& ttj, + Vec6d& res) { - int numEdges = poseGraph.getNumEdges(); - int numNodes = poseGraph.getNumNodes(); - if (numEdges == 0) + // 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) { - CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); - return; + // 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; } - ceres::LossFunction* lossFunction = nullptr; - // TODO: Experiment with SE3 parameterization - ceres::LocalParameterization* quatLocalParameterization = - new ceres::EigenQuaternionParameterization; + return res.ddot(res); +} + + +double PoseGraphImpl::calcEnergy() const +{ + return calcEnergyNodes(nodes); +} + - for (int currEdgeNum = 0; currEdgeNum < numEdges; ++currEdgeNum) +// estimate current energy +double PoseGraphImpl::calcEnergyNodes(const std::map& newNodes) const +{ + double totalErr = 0; + for (const auto& e : 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; + Pose3d srcP = newNodes.at(e.sourceNodeId).pose; + Pose3d tgtP = newNodes.at(e.targetNodeId).pose; - const Matx66f& informationMatrix = currEdge.information; + 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 = */ false, sqj, stj, tqj, ttj, res); + + totalErr += err; + } + return totalErr * 0.5; +}; - ceres::CostFunction* costFunction = Pose3dErrorFunctor::create( - Pose3d(currEdge.transformation.rotation(), currEdge.transformation.translation()), - informationMatrix); - 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); +#if defined(HAVE_EIGEN) + +// 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) +{ + 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; +}; + - for (int currNodeId = 0; currNodeId < numNodes; ++currNodeId) +// 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) { - PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); - if (currNode.isPoseFixed()) + Point2i bpt = ijv.first; + Matx66d& m = ijv.second; + for (int i = 0; i < 6; i++) { - problem.SetParameterBlockConstant(currNode.se3Pose.t.data()); - problem.SetParameterBlockConstant(currNode.se3Pose.r.coeffs().data()); + 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]; + } } -#endif -void Optimizer::optimize(PoseGraph& poseGraph) + +int PoseGraphImpl::optimize(const cv::TermCriteria& tc) { - PoseGraph poseGraphOriginal = poseGraph; + if (!isValid()) + { + CV_LOG_INFO(NULL, "Invalid PoseGraph that is either not connected or has invalid nodes"); + return -1; + } + + size_t numNodes = getNumNodes(); + size_t numEdges = getNumEdges(); + + // Allocate indices for nodes + std::vector placesIds; + std::map idToPlace; + for (const auto& ni : nodes) + { + if (!ni.second.isFixed) + { + idToPlace[ni.first] = placesIds.size(); + placesIds.push_back(ni.first); + } + } + + size_t nVarNodes = placesIds.size(); + if (!nVarNodes) + { + CV_LOG_INFO(NULL, "PoseGraph contains no non-constant nodes, skipping optimization"); + return -1; + } - if (!poseGraphOriginal.isValid()) + if (numEdges == 0) { - CV_Error(Error::StsBadArg, - "Invalid PoseGraph that is either not connected or has invalid nodes"); - return; + CV_LOG_INFO(NULL, "PoseGraph has no edges, no optimization to be done"); + return -1; } - int numNodes = poseGraph.getNumNodes(); - int numEdges = poseGraph.getNumEdges(); - 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); + std::vector jtb(nVars); + + double energy = calcEnergyNodes(nodes); + double oldEnergy = energy; + + CV_LOG_INFO(NULL, "#s" << " energy: " << energy); + + // options + // stop conditions + 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 = 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; + 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 jacobian scaling + std::vector di(nVars); + + double lmUpFactor = initialLmUpFactor; + double lambdaLevMarq = initialLambdaLevMarq; + + unsigned int iter = 0; + bool done = false; + while (!done) + { + jtj.clear(); + std::fill(jtb.begin(), jtb.end(), 0.0); + + // caching nodes jacobians + std::vector> cachedJac; + for (auto id : placesIds) + { + 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()), + concatHor(Matx33d(), Matx33d::eye())); + cachedJac.push_back(j); + } + + // fill jtj and jtb + for (const auto& e : edges) + { + size_t srcId = e.sourceNodeId, dstId = e.targetNodeId; + const Node& srcNode = nodes.at(srcId); + const Node& dstNode = nodes.at(dstId); -#if defined(CERES_FOUND) && defined(HAVE_EIGEN) - ceres::Problem problem; - createOptimizationProblem(poseGraph, problem); + Pose3d srcP = srcNode.pose; + Pose3d tgtP = dstNode.pose; + bool srcFixed = srcNode.isFixed; + bool dstFixed = dstNode.isFixed; - ceres::Solver::Options options; - options.max_num_iterations = 100; - options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + Vec6d res; + Matx stj, ttj; + Matx sqj, tqj; + poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, + /* needJacobians = */ true, sqj, stj, tqj, ttj, res); - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); + size_t srcPlace = (size_t)(-1), dstPlace = (size_t)(-1); + Matx66d sj, tj; + if (!srcFixed) + { + srcPlace = idToPlace.at(srcId); + sj = concatHor(sqj, stj) * cachedJac[srcPlace]; + + jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; + + Vec6f jtbSrc = sj.t() * res; + for (int i = 0; i < 6; i++) + { + jtb[6 * srcPlace + i] += -jtbSrc[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++) + { + jtb[6 * dstPlace + i] += -jtbDst[i]; + } + } + + if (!(srcFixed || dstFixed)) + { + Matx66d sjttj = sj.t() * tj; + jtj.refBlock(srcPlace, dstPlace) += sjttj; + jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); + } + } - std::cout << summary.FullReport() << '\n'; + CV_LOG_INFO(NULL, "#LM#s" << " energy: " << energy); + + // 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 (size_t i = 0; i < nVars; i++) + { + double ds = sqrt(jtj.valElem(i, i)) + 1.0; + di[i] = 1.0 / ds; + } + } + + doJacobiScaling(jtj, jtb, di); + } + + double gradientMax = 0.0; + // gradient max + 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 (size_t i = 0; i < nVars; i++) + { + diag[i] = jtj.valElem(i, i); + } + + // Solve using LevMarq and get delta transform + bool enoughLm = false; + + decltype(nodes) tempNodes = nodes; + + while (!enoughLm && !done) + { + // form LevMarq matrix + std::vector lmDiag(nVars); + for (size_t i = 0; i < nVars; i++) + { + double v = diag[i]; + double ld = std::min(max(v * lambdaLevMarq, minDiag), maxDiag); + lmDiag[i] = ld; + jtj.refElem(i, i) = v + ld; + } + + CV_LOG_INFO(NULL, "sparse solve..."); + + // use double or convert everything to float + std::vector x; + bool solved = jtj.sparseSolve(jtb, x, false); + + CV_LOG_INFO(NULL, (solved ? "OK" : "FAIL")); + + double costChange = 0.0; + double jacCostChange = 0.0; + double stepQuality = 0.0; + double xNorm2 = 0.0; + if (solved) + { + jacCostChange = calcJacCostChange(jtb, x, lmDiag); + + // x squared norm + for (size_t i = 0; i < nVars; i++) + { + xNorm2 += x[i] * x[i]; + } + + // undo jacobi scaling + if (jacobiScaling) + { + for (size_t i = 0; i < nVars; i++) + { + x[i] *= di[i]; + } + } + + tempNodes = nodes; + + // Update temp nodes using x + 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]).pose; + + p.q = Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.q; + p.t += deltaTrans; + } + + // calc energy with temp nodes + energy = calcEnergyNodes(tempNodes); + + costChange = oldEnergy - energy; + + stepQuality = costChange / jacCostChange; + + 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) + { + // failed to optimize, increase lambda and repeat + + lambdaLevMarq *= lmUpFactor; + lmUpFactor *= 2.0; + + CV_LOG_INFO(NULL, "LM goes up, lambda: " << lambdaLevMarq << ", old energy: " << oldEnergy); + } + else + { + // optimized successfully, decrease lambda and set variables for next iteration + enoughLm = true; + + lambdaLevMarq *= std::max(1.0 / initialLmDownFactor, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + lmUpFactor = initialLmUpFactor; + + smallGradient = (gradientMax < minGradientTolerance); + smallStep = (xNorm2 < stepNorm2Tolerance); + smallEnergyDelta = (costChange / energy < relEnergyDeltaTolerance); + + nodes = tempNodes; + + CV_LOG_INFO(NULL, "#" << iter << " energy: " << energy); + + oldEnergy = energy; + + CV_LOG_INFO(NULL, "LM goes down, lambda: " << lambdaLevMarq << " step quality: " << stepQuality); + } + + iter++; + + tooLong = (iter >= maxIterations); + + done = ( (checkIterations && tooLong) || smallGradient || smallStep || (checkEps && 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) + CV_LOG_INFO(NULL, "Finish reason: gradient max val dropped below threshold"); + if (smallStep) + CV_LOG_INFO(NULL, "Finish reason: step size dropped below threshold"); + if (smallEnergyDelta) + 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); +} - std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; #else - CV_Error(Error::StsNotImplemented, "Ceres and Eigen required for Pose Graph optimization"); +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 diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp deleted file mode 100644 index e8b7c34c53b..00000000000 --- a/modules/rgbd/src/pose_graph.hpp +++ /dev/null @@ -1,321 +0,0 @@ -#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 - -#if defined(CERES_FOUND) -#include -#endif - -namespace cv -{ -namespace kinfu -{ -/*! \class GraphNode - * \brief Defines a node/variable that is optimizable in a posegraph - * - * Detailed description - */ -#if defined(HAVE_EIGEN) -struct Pose3d -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Eigen::Vector3d t; - Eigen::Quaterniond r; - - Pose3d() - { - t.setZero(); - r.setIdentity(); - }; - Pose3d(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) - : t(translation), r(Eigen::Quaterniond(rotation)) - { - normalizeRotation(); - } - - Pose3d(const Matx33d& rotation, const Vec3d& translation) - { - Eigen::Matrix3d R; - cv2eigen(rotation, R); - cv2eigen(translation, t); - r = Eigen::Quaterniond(R); - normalizeRotation(); - } - - 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); - } - - // 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(); - return out; - } - - inline Pose3d& operator*=(const Pose3d& otherPose) - { - t += otherPose.t; - r *= otherPose.r; - normalizeRotation(); - return *this; - } - - inline Pose3d inverse() const - { - Pose3d out; - out.r = r.conjugate(); - out.t = out.r * (t * -1.0); - return out; - } - - inline void normalizeRotation() - { - if (r.w() < 0) - r.coeffs() *= -1.0; - r.normalize(); - } -}; -#endif - -struct PoseGraphNode -{ - public: - 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; - - int getId() const { return nodeId; } - inline Affine3f getPose() const - { - return pose; - } - 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; - } -#endif - void setFixed(bool val = true) { isFixed = val; } - bool isPoseFixed() const { return isFixed; } - - public: - int nodeId; - bool isFixed; - Affine3f pose; -#if defined(HAVE_EIGEN) - Pose3d se3Pose; -#endif -}; - -/*! \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()) - : sourceNodeId(_sourceNodeId), - targetNodeId(_targetNodeId), - transformation(_transformation), - information(_information) - { - } - virtual ~PoseGraphEdge() = default; - - 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; - } - - public: - int sourceNodeId; - int targetNodeId; - Affine3f transformation; - Matx66f information; -}; - -//! @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: - typedef std::vector NodeVector; - typedef std::vector EdgeVector; - - 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 addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } - - bool nodeExists(int nodeId) const - { - return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { - return currNode.getId() == nodeId; - }) != nodes.end(); - } - - bool isValid() const; - - int getNumNodes() const { return int(nodes.size()); } - int getNumEdges() const { return int(edges.size()); } - - public: - NodeVector nodes; - EdgeVector edges; -}; - -namespace Optimizer -{ -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::Map> sourceQuat(_pSourceQuat); - Eigen::Map> targetQuat(_pTargetQuat); - Eigen::Map> residual(_pResidual); - - Eigen::Quaternion targetQuatInv = targetQuat.conjugate(); - - Eigen::Quaternion relativeQuat = targetQuatInv * sourceQuat; - Eigen::Matrix relativeTrans = targetQuatInv * (targetTrans - sourceTrans); - - //! Definition should actually be relativeQuat * poseMeasurement.r.conjugate() - Eigen::Quaternion deltaRot = - poseMeasurement.r.template cast() * relativeQuat.conjugate(); - - residual.template block<3, 1>(0, 0) = relativeTrans - poseMeasurement.t.template cast(); - 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; -}; -#endif - -} // namespace Optimizer - -} // namespace kinfu -} // namespace cv -#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ 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" diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 0e607af639d..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 @@ -24,7 +25,7 @@ namespace kinfu * \class BlockSparseMat * Naive implementation of Sparse Block Matrix */ -template +template struct BlockSparseMat { struct Point2iHash @@ -41,55 +42,78 @@ 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) + void clear() { - Point2i p(i, j); + ijValue.clear(); + } + + inline 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() + 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); + } + + inline 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; + } + + 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)); + 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; - 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) = valElem(i, i); } return diag; } - float& refElem(int i, int j) - { - Point2i ib(i / blockM, j / blockN), iv(i % blockM, 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 (auto ijv : ijValue) + for (const auto& ijv : ijValue) { 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++) { - float val = vblock(i, j); + _Tp val = vblock((int)i, (int)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>((int)(blockM * xb + i), (int)(blockN * yb + j), val)); } } } @@ -101,59 +125,76 @@ struct BlockSparseMat return EigenMat; } #endif - size_t nonZeroBlocks() const { return ijValue.size(); } + inline size_t nonZeroBlocks() const { return ijValue.size(); } - static constexpr float NON_ZERO_VAL_THRESHOLD = 0.0001f; - int nBlocks; - IDtoBlockValueMap ijValue; -}; + 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; + } -//! 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) -{ - bool result = false; -#if defined(HAVE_EIGEN) - Eigen::SparseMatrix bigA = H.toEigen(); - Eigen::VectorXf bigB; - cv2eigen(B, bigB); + return *this; + } - Eigen::SparseMatrix bigAtranspose = bigA.transpose(); - if(!bigA.isApprox(bigAtranspose)) +#if defined(HAVE_EIGEN) + //! 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 result; - } + 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; - result = false; - } - else - { - Eigen::VectorXf solutionX = solver.solve(bigB); - Eigen::VectorXf predBEigen = bigA * solutionX; + Eigen::SimplicialLDLT> solver; + + solver.compute(bigA); if (solver.info() != Eigen::Success) { - std::cout << "failed to eigen-solve" << std::endl; - result = false; + CV_LOG_INFO(NULL, "Failed to eigen-decompose"); + return false; } else { - eigen2cv(solutionX, X); - eigen2cv(predBEigen, predB); - result = true; + Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB); + if (solver.info() != Eigen::Success) + { + CV_LOG_INFO(NULL, "Failed to eigen-solve"); + return false; + } + else + { + eigen2cv(solutionX, X); + if (predB.needed()) + { + Eigen::Matrix<_Tp, -1, 1> 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"); + bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) const + { + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); + } #endif - return result; -} + + static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001); + size_t nBlocks; + IDtoBlockValueMap ijValue; +}; + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index b40023b6a56..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,10 +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) { @@ -507,34 +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(); - PoseGraphEdge 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) { - PoseGraphNode 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 PoseGraphNode& 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 new file mode 100644 index 00000000000..a675e343c27 --- /dev/null +++ b/modules/rgbd/test/test_pose_graph.cpp @@ -0,0 +1,151 @@ +// 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; + +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) +{ + Ptr pg = kinfu::detail::PoseGraph::create(); + + // 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; + + 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; + } + + return pg; +} + + +TEST( PoseGraph, sphereG2O ) +{ + // 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. + // 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() + "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 + + int iters = pg->optimize(); + + ASSERT_GE(iters, 0); + ASSERT_LE(iters, 36); // should converge in 36 iterations + + double energy = pg->calcEnergy(); + + ASSERT_LE(energy, 1.47723e+06); // should converge to 1.47722e+06 or less + + // 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"; + 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(); + } +#else + throw SkipTestException("Build with Eigen required for pose graph optimization"); +#endif +} + + +}} // namespace \ No newline at end of file