From a384a0d4b72d6687b63427280218e05c73c4a1b3 Mon Sep 17 00:00:00 2001 From: wqhot Date: Sat, 10 Oct 2020 17:11:32 +0800 Subject: [PATCH 1/4] removing arma --- .gitignore | 4 +- .vscode/settings.json | 60 ++++++++++++++++ CMakeLists.txt | 3 +- src/demo/demo.cpp | 15 ++-- src/softposit/softposit.cpp | 138 ++++++++++++++++++++---------------- src/softposit/softposit.hpp | 21 ++++-- 6 files changed, 161 insertions(+), 80 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 0124c8b..20abff7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ -<<<<<<< HEAD /.clang_complete /GPATH /GRTAGS @@ -10,8 +9,6 @@ build *.pdf *.tex *.fdb_latexmk -======= ->>>>>>> 9ee4789... Initial commit # Compiled Object files *.slo *.lo @@ -40,3 +37,4 @@ build *.exe *.out *.app +.vscode/c_cpp_properties.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..ba92023 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "*.tcc": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "core": "cpp", + "dense": "cpp", + "armadillo": "cpp" + } +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e4eb81..2b9f95a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -std=c++1y") if (APPLE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") -endif +endif() set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}) set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}) @@ -52,6 +52,7 @@ include_directories( ${PROJECT_BINARY_DIR} ${Boost_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} + "/usr/include/eigen3/" ) add_definitions("-D_USE_MATH_DEFINES") diff --git a/src/demo/demo.cpp b/src/demo/demo.cpp index bd232e8..c2ec5a9 100644 --- a/src/demo/demo.cpp +++ b/src/demo/demo.cpp @@ -9,8 +9,8 @@ int main() std::string name; std::cout<<"hello, "< imagePts; - std::vector worldPts; + std::vector imagePts; + std::vector worldPts; std::vector raw_imagePts { 612, @@ -26,7 +26,7 @@ int main() std::cout<<"load image points"<(0, 0) << 0, 1, 0, -1, 0, 0, 0, 0, 1; + initpose.rot = initpose.rot.transpose(); + initpose.trans = bloody::point3d_type_eigen{0, 0, 30}; auto maybe_pose = softposit( imagePts, diff --git a/src/softposit/softposit.cpp b/src/softposit/softposit.cpp index 91a381d..4123ddc 100644 --- a/src/softposit/softposit.cpp +++ b/src/softposit/softposit.cpp @@ -4,14 +4,18 @@ namespace bloody{ using stat_info = arma::vec5; - std::tuple maxPosRatio(arma::mat assignMat); + using stat_info_eigen = Eigen::Matrix; +// std::tuple maxPosRatio(arma::mat assignMat); + std::tuple, Eigen::MatrixXd> + maxPosRatio(Eigen::MatrixXd assignMat); arma::mat sinkhornSlack(arma::mat M); - arma::mat sinkhornImp(arma::mat M); + arma::mat sinkhornSlack_eigen(arma::mat M); + Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M); int numMatches(arma::mat assignMat); boost::optional< std::tuple > softposit( - const std::vector& imagePts, - const std::vector& worldPts, + const std::vector& imagePts, + const std::vector& worldPts, const Param_type& param, const Pose_type& initpose, boost::optional maybe_caminfo) @@ -21,12 +25,12 @@ namespace bloody{ std::vector stats; - auto alpha = 9.21*pow(param.noiseStd,2) + 1; - auto maxDelta = sqrt(alpha)/2; // Max allowed error per world point. + auto alpha = 9.21 * std::pow(param.noiseStd, 2) + 1; + auto maxDelta = std::sqrt(alpha) / 2; // 每个世界坐标所允许的最大误差 - auto betaFinal = 0.5; // Terminate iteration when beta == betaFinal. - auto betaUpdate = 1.05; // Update rate on beta. - auto epsilon0 = 0.01; // Used to initialize assignement matrix. + auto betaFinal = 0.5; // 最终迭代目标 + auto betaUpdate = 1.05; // 迭代率 + auto epsilon0 = 0.01; // 用于初始化任务矩阵 auto maxCount = 1; auto minBetaCount = 20; @@ -34,53 +38,55 @@ namespace bloody{ auto nbWorldPts = worldPts.size(); auto minNbPts = std::min(nbImagePts, nbWorldPts); - auto maxNbPts = nbImagePts+nbWorldPts - minNbPts; + auto maxNbPts = nbImagePts + nbWorldPts - minNbPts; - auto scale = 1.0/(maxNbPts + 1); + auto scale = 1.0 / (maxNbPts + 1); - std::vector _centeredImage(imagePts.size()); + std::vector _centeredImage(imagePts.size()); CamInfo_type caminfo; if (maybe_caminfo) caminfo = *maybe_caminfo; else{ caminfo.focalLength = 1; - caminfo.center = point2di_type{0, 0}; + caminfo.center = point2di_type_eigen{0, 0}; } std::cout<<"init: "< centeredImage; - arma::mat centeredImage = arma::zeros(_centeredImage.size(),2); - - for (int j=0; j(i, 0) << _centeredImage[i].x(), _centeredImage[i].y(); } - std::cout<<"centered image :"<(worldPts.size(), 4).eval(); - for (int i=0; i homogeneousWorldPts; + for (int i = 0; i < worldPts.size(); ++i) { - homogeneousWorldPts.row(i) = arma::rowvec{worldPts[i][0], worldPts[i][1], worldPts[i][2],1}; + homogeneousWorldPts.block<1, 4>(i, 0) << worldPts[i][0], worldPts[i][1], worldPts[i][2],1; } - std::cout<<"begin to make world point homogeneous:" << homogeneousWorldPts <(nbImagePts, 1); + Eigen::VectorXd imageOnes = Eigen::MatrixXd::Ones(nbImagePts, 1); int debug_loop = 0; while (beta < betaFinal && !assignConverged) { std::cout<(nbImagePts+1); - assignMat.row(nbImagePts) = scale * arma::ones(nbWorldPts+1); + + assignMat.block(0, 0, nbImagePts-1, nbWorldPts-1) = scale * (-beta*(distMat - alpha * Eigen::MatrixXd::Ones(distMat.rows(), distMat.cols()))).array().exp(); + // assignMat(arma::span(0, nbImagePts-1), arma::span(0, nbWorldPts-1)) = scale*arma::exp(-beta*(distMat - alpha)); + assignMat.col(nbWorldPts) = scale * Eigen::VectorXd::Ones(nbImagePts+1); + assignMat.row(nbImagePts) = scale * Eigen::VectorXd::Ones(nbWorldPts+1).transpose(); std::cout<<"assign befor sinkhorn:"< arma::umat posmax; arma::mat ratios; std::tie(posmax, ratios) = maxPosRatio(M); @@ -365,26 +375,28 @@ namespace bloody{ } - std::tuple maxPosRatio(arma::mat assignMat) + std::tuple, Eigen::MatrixXd> + maxPosRatio(Eigen::MatrixXd assignMat) { // function [pos, ratios] = maxPosRatio(assignMat) - arma::umat pos; - arma::mat ratios; + Eigen::Matrix pos; + Eigen::MatrixXd ratios; - auto nrows = assignMat.n_rows; //size(assignMat,1); - auto ncols = assignMat.n_cols; //size(assignMat,2); + auto nrows = assignMat.rows(); //size(assignMat,1); + auto ncols = assignMat.cols(); //size(assignMat,2); auto nimgpnts = nrows - 1; auto nmodpnts = ncols - 1; - + arma::mat test; + // 按列遍历 for (auto k=0u; k other_cols; - for (int i=0; i other_cols; + for (int i=0; i assignMat.submat(arma::uvec{imax},arma::uvec(other_cols))))){ pos = arma::join_cols(pos, arma::umat(arma::urowvec{imax, k})); diff --git a/src/softposit/softposit.hpp b/src/softposit/softposit.hpp index 26af6d1..1c18b43 100644 --- a/src/softposit/softposit.hpp +++ b/src/softposit/softposit.hpp @@ -2,6 +2,8 @@ #define uu_recreate_wheel_imp_SOFTPOSIT_H #include +#include +#include #include #include @@ -21,6 +23,13 @@ namespace bloody{ using point2d_type = arma::vec2; using point3d_type = arma::vec3; + using vec3_type_eigen = Eigen::Vector3d; + using mat3_type_eigen = Eigen::Matrix3d; + + using point2di_type_eigen = Eigen::Vector2i; + using point2d_type_eigen = Eigen::Vector3d; + using point3d_type_eigen = Eigen::Vector3d; + using match_type = std::vector >; struct Param_type{ @@ -29,20 +38,20 @@ namespace bloody{ }; struct Pose_type{ - mat3_type rot; - vec3_type trans; + mat3_type_eigen rot; + vec3_type_eigen trans; }; struct CamInfo_type{ - float focalLength; - point2di_type center; + double focalLength; + point2di_type_eigen center; }; // at least it will find something boost::optional< std::tuple > softposit( - const std::vector& imagePts, - const std::vector& worldPts, + const std::vector& imagePts, + const std::vector& worldPts, const Param_type& param, const Pose_type& initpose, boost::optional maybe_caminfo); From c0f8681700d6fbcb300ef15e814dbdd0e09ba3ee Mon Sep 17 00:00:00 2001 From: wqhot Date: Mon, 12 Oct 2020 17:33:25 +0800 Subject: [PATCH 2/4] removing arma:part2 --- .vscode/launch.json | 27 +++++ .vscode/tasks.json | 26 ++++ CMakeLists.txt | 11 +- src/demo/demo.cpp | 3 +- src/softposit/softposit.cpp | 236 +++++++++++++++++++----------------- 5 files changed, 185 insertions(+), 118 deletions(-) create mode 100644 .vscode/launch.json create mode 100644 .vscode/tasks.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..d9ea4cc --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,27 @@ +{ + // 使用 IntelliSense 了解相关属性。 + // 悬停以查看现有属性的描述。 + // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "(gdb) 启动", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/build/softpositdemo", + "args": [], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "为 gdb 启用整齐打印", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + } + ] +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..6f87cd5 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,26 @@ +{ + "tasks": [ + { + "type": "shell", + "label": "C/C++: g++ build active file", + "command": "/usr/bin/g++", + "args": [ + "-g", + "${file}", + "-o", + "${fileDirname}/${fileBasenameNoExtension}" + ], + "options": { + "cwd": "${workspaceFolder}" + }, + "problemMatcher": [ + "$gcc" + ], + "group": { + "kind": "build", + "isDefault": true + } + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b9f95a..3eed566 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,14 +2,21 @@ cmake_minimum_required(VERSION 3.0) project(SoftPosit) +set(CMAKE_BUILD_TYPE "DEBUG") +if (CMAKE_BUILD_TYPE STREQUAL "RELEASE") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-std=c++14 -O3 -pthread -fopenmp -s") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -pthread -fopenmp -s") +else(CMAKE_BUILD_TYPE STREQUAL "RELEASE") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-std=c++14 -O0 -g -Wall -pthread") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O0 -g -Wall -pthread") +endif(CMAKE_BUILD_TYPE STREQUAL "RELEASE") + IF(CMAKE_VERSION VERSION_EQUAL "3.0.0" OR CMAKE_VERSION VERSION_GREATER "3.0.0") CMAKE_POLICY(SET CMP0045 OLD) # suppress set_target_properties empty target warnning CMAKE_POLICY(SET CMP0026 OLD) # suppress set_target_properties read LOCATION warnning ENDIF() # use libc++ for osx -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -std=c++1y") - if (APPLE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") endif() diff --git a/src/demo/demo.cpp b/src/demo/demo.cpp index c2ec5a9..f1ea26e 100644 --- a/src/demo/demo.cpp +++ b/src/demo/demo.cpp @@ -60,8 +60,7 @@ int main() bloody::Pose_type initpose; - initpose.rot.block<3, 3>(0, 0) << 0, 1, 0, -1, 0, 0, 0, 0, 1; - initpose.rot = initpose.rot.transpose(); + initpose.rot << 0, -1, 0, 1, 0, 0, 0, 0, 1; initpose.trans = bloody::point3d_type_eigen{0, 0, 30}; auto maybe_pose = softposit( diff --git a/src/softposit/softposit.cpp b/src/softposit/softposit.cpp index 4123ddc..05ea344 100644 --- a/src/softposit/softposit.cpp +++ b/src/softposit/softposit.cpp @@ -11,7 +11,7 @@ namespace bloody{ arma::mat sinkhornSlack(arma::mat M); arma::mat sinkhornSlack_eigen(arma::mat M); Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M); - int numMatches(arma::mat assignMat); + int numMatches(Eigen::MatrixXd assignMat); boost::optional< std::tuple > softposit( const std::vector& imagePts, @@ -53,37 +53,30 @@ namespace bloody{ } std::cout<<"init: "< centeredImage; + centeredImage.resize(imagePts.size(), Eigen::NoChange); - for (size_t i = 0; i < _centeredImage.size(); ++i) + for (size_t i = 0; i < imagePts.size(); ++i) { - centeredImage.block<1, 2>(i, 0) << _centeredImage[i].x(), _centeredImage[i].y(); + centeredImage.block<1, 2>(i, 0) << (imagePts[i].x() - (double)caminfo.center.x()) / caminfo.focalLength, + (imagePts[i].y() - (double)caminfo.center.y()) / caminfo.focalLength; } - // std::cout<<"centered image :"< homogeneousWorldPts; + homogeneousWorldPts.resize(worldPts.size(), Eigen::NoChange); + for (int i = 0; i < worldPts.size(); ++i) { homogeneousWorldPts.block<1, 4>(i, 0) << worldPts[i][0], worldPts[i][1], worldPts[i][2],1; } - // std::cout<<"begin to make world point homogeneous:" << homogeneousWorldPts <(4, 4); + Eigen::VectorXd summedByColAssign = assignMat.block(0, 0, nbImagePts, nbWorldPts).colwise().sum(); + Eigen::Matrix4d sumSkSkT = Eigen::Matrix4d::Zero(4, 4); for(auto k = 0; k 1e10){ + Eigen::JacobiSVD svd_sumSKSkT(sumSkSkT); + double cond = svd_sumSKSkT.singularValues()(0) / svd_sumSKSkT.singularValues()(svd_sumSKSkT.singularValues().size()-1); + if (cond > 1e10){ std::cout<<"sumSkSkT is ill-conditioned, termininating search."< X; - X.col(0) = r1T(arma::span(0,2)); - X.col(1) = r2T(arma::span(0,2)); + X.col(0) = r1T.head(3); + X.col(1) = r2T.head(3); std::cout<<"svd"<> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // arma::svd(U,s,V, X); + Eigen::Matrix II; + II << 1, 0, 0, 1, 0, 0 ; + s = svd.singularValues(); + Eigen::MatrixXd A = svd.matrixU() * II * svd.matrixV().transpose(); r1 = A.col(0); r2 = A.col(1); - r3 = arma::cross(r1,r2); + r3 = r1.cross(r2); Tz = 2 / (s(0) + s(1)); Tx = r1T(3) * Tz; Ty = r2T(3) * Tz; - auto r3T= arma::vec{r3[0], r3[1], r3[2], Tz}; + auto r3T= Eigen::Vector4d{r3[0], r3[1], r3[2], Tz}; std::cout<<"svd"<{ beta ,delta ,double(numMatchPts)/nbWorldPts , double(sumNonslack)/nbWorldPts, - arma::accu(arma::square(r1T-r1Tprev)) + arma::accu(arma::square(r2T-r2Tprev)) + ((r1T-r1Tprev).array().square() + (r2T-r2Tprev).array().square()).sum() }; std::cout<<"keep log"< minBetaCount; - pose.trans = arma::vec{Tx, Ty, Tz}; - pose.rot.row(0) = r1.t(); - pose.rot.row(1) = r2.t(); - pose.rot.row(2) = r3.t(); + pose.trans = Eigen::Vector3d{Tx, Ty, Tz}; + pose.rot.row(0) = r1.transpose(); + pose.rot.row(1) = r2.transpose(); + pose.rot.row(2) = r3.transpose(); foundPose = (delta < maxDelta && betaCount > minBetaCount); @@ -294,26 +292,30 @@ namespace bloody{ } - int numMatches(arma::mat assignMat) + int numMatches(Eigen::MatrixXd assignMat) { int num = 0; - auto nimgpnts = assignMat.n_rows - 1; - auto nmodpnts = assignMat.n_cols - 1; + auto nimgpnts = assignMat.rows() - 1; + auto nmodpnts = assignMat.cols() - 1; for (int k = 0; k< nmodpnts; ++k){ - arma::uword imax; - auto vmax = assignMat.col(k).max(imax); + unsigned long long imax; + auto vmax = assignMat.col(k).maxCoeff(&imax); - if(imax == assignMat.n_rows - 1) continue; // Slack value is maximum in this column. + if(imax == assignMat.rows() - 1) continue; // Slack value is maximum in this column. - std::vector other_cols; - other_cols.reserve(assignMat.n_cols-1); - for (int i=0; i other_cols; + other_cols.reserve(assignMat.cols()-1); + for (int i=0; i assignMat.submat(arma::uvec{imax},arma::uvec(other_cols))))) + unsigned long long kmax; + assignMat.row(imax).maxCoeff(&kmax); + if (kmax == k){ num = num + 1; // This value is maximal in its row & column. + } + } return num; } @@ -333,8 +335,8 @@ namespace bloody{ // Get the positions and ratios to slack of the nonslack elements that are maximal in their row and column. // Eigen::Matrix - arma::umat posmax; - arma::mat ratios; + Eigen::Matrix posmax; + Eigen::Matrix ratios; std::tie(posmax, ratios) = maxPosRatio(M); std::cout<<"postmax, rations"<(nbRows) * McolSums ; - M = M / McolSumsRep; + + Eigen::MatrixXd McolSumsRep = Eigen::VectorXd::Ones(nbRows) * McolSums.transpose(); + M = M.cwiseQuotient(McolSumsRep); // Fix values in the slack column. - for (auto i=0; i< posmax.n_rows; ++i){ + for (auto i=0; i< posmax.rows(); ++i){ M(posmax(i,0), nbCols-1) = ratios(i,0)*M(posmax(i,0),posmax(i,1)); } // Row normalization (except outlier row - do not normalize col slacks against each other) - arma::vec MrowSums = arma::sum(M, 1); // Column vector. + Eigen::VectorXd MrowSums = M.rowwise().sum(); // Column vector. MrowSums(nbRows-1) = 1; // Don't normalize slack row terms against each other. - auto MrowSumsRep = MrowSums * arma::ones(nbCols); - M = M / MrowSumsRep; + auto MrowSumsRep = MrowSums * Eigen::VectorXd::Ones(nbCols).transpose(); + M = M.cwiseQuotient(MrowSumsRep); // Fix values in the slack row. - for (auto i=0; i< posmax.n_rows; ++i){ + for (auto i=0; i< posmax.rows(); ++i){ M(nbRows-1,posmax(i,1)) = ratios(i,1)*M(posmax(i,0),posmax(i,1)); } iNumSinkIter=iNumSinkIter+1; - fMdiffSum=arma::accu(arma::abs(M-Mprev)); + fMdiffSum=(M-Mprev).cwiseAbs().sum(); } normalizedMat = M; @@ -379,8 +381,10 @@ namespace bloody{ maxPosRatio(Eigen::MatrixXd assignMat) { // function [pos, ratios] = maxPosRatio(assignMat) - Eigen::Matrix pos; - Eigen::MatrixXd ratios; + Eigen::Matrix pos; + Eigen::Matrix ratios; + std::vector pos_; + std::vector ratios_; auto nrows = assignMat.rows(); //size(assignMat,1); auto ncols = assignMat.cols(); //size(assignMat,2); @@ -390,54 +394,58 @@ namespace bloody{ // 按列遍历 for (auto k=0u; k other_cols; - for (int i=0; i assignMat.submat(arma::uvec{imax},arma::uvec(other_cols))))){ - pos = arma::join_cols(pos, arma::umat(arma::urowvec{imax, k})); - +// 检查列中的最大值在其行中是否为最大值. + // 在其行中是最大值 + unsigned long long kmax; + auto _t = assignMat.row(imax).maxCoeff(&kmax); + if (kmax == k) + { + pos_.push_back(imax); + pos_.push_back(k); // Compute the ratios to row and column slack values. auto rr = assignMat(imax,ncols-1)/assignMat(imax,k); auto cr = assignMat(nrows-1,k)/assignMat(imax,k); - ratios = arma::join_cols(ratios, arma::mat(arma::rowvec{rr, cr})); + ratios_.push_back(rr); + ratios_.push_back(cr); } - } + } + pos = Eigen::Map, Eigen::Unaligned>(pos_.data(), pos_.size()); + ratios = Eigen::Map, Eigen::Unaligned>(ratios_.data(), ratios_.size()); return std::make_tuple(pos, ratios); } - void softposit(float* rot, float* trans, int* foundPose, int* _imagePts, float* _worldPts, int nbImagePts, int nbWorldPts, float beta0, float noiseStd, float* initRot, float* initTrans, float* focalLength, int* center) - { - - std::vector imagePts; - std::vector worldPts; - - for (uint i=0u; i imagePts; + // std::vector worldPts; + + // for (uint i=0u; i Date: Tue, 13 Oct 2020 15:28:03 +0800 Subject: [PATCH 3/4] refact: remove arma and boost use eigen3 --- CMakeLists.txt | 21 +- src/demo/demo.cpp | 131 +++--- src/softposit/softposit.cpp | 821 ++++++++++++++++++------------------ src/softposit/softposit.hpp | 94 ++--- 4 files changed, 516 insertions(+), 551 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3eed566..8010f02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,15 +2,6 @@ cmake_minimum_required(VERSION 3.0) project(SoftPosit) -set(CMAKE_BUILD_TYPE "DEBUG") -if (CMAKE_BUILD_TYPE STREQUAL "RELEASE") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-std=c++14 -O3 -pthread -fopenmp -s") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -pthread -fopenmp -s") -else(CMAKE_BUILD_TYPE STREQUAL "RELEASE") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-std=c++14 -O0 -g -Wall -pthread") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O0 -g -Wall -pthread") -endif(CMAKE_BUILD_TYPE STREQUAL "RELEASE") - IF(CMAKE_VERSION VERSION_EQUAL "3.0.0" OR CMAKE_VERSION VERSION_GREATER "3.0.0") CMAKE_POLICY(SET CMP0045 OLD) # suppress set_target_properties empty target warnning CMAKE_POLICY(SET CMP0026 OLD) # suppress set_target_properties read LOCATION warnning @@ -47,24 +38,18 @@ if (APPLE) set(ALL_LIBS ${Accelerate_LIBRARIES}) endif() +# find Eigen3 +find_package(Eigen3 3.0.0 REQUIRED) -# find boost -find_package(Boost) -link_directories(${Boost_LIBRARY_DIRS}) - -# armadillo -find_package(Armadillo REQUIRED) include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ${PROJECT_BINARY_DIR} - ${Boost_INCLUDE_DIRS} - ${ARMADILLO_INCLUDE_DIRS} "/usr/include/eigen3/" ) add_definitions("-D_USE_MATH_DEFINES") -set(ALL_LIBS ${ALL_LIBS} ${Boost_LIBRARIES} ${ARMADILLO_LIBRARIES}) +set(ALL_LIBS ${ALL_LIBS}) add_subdirectory(src) diff --git a/src/demo/demo.cpp b/src/demo/demo.cpp index f1ea26e..effe757 100644 --- a/src/demo/demo.cpp +++ b/src/demo/demo.cpp @@ -6,79 +6,82 @@ int main() { - std::string name; - std::cout<<"hello, "< imagePts; - std::vector worldPts; + std::vector imagePts; + std::vector worldPts; - std::vector raw_imagePts { - 612, - 117, - 486, - 145, - 567, - 206, - 476, - 234, - 441, - 329}; + std::vector raw_imagePts{ + 612, + 117, + 486, + 145, + 567, + 206, + 476, + 234, + 441, + 329}; - std::cout<<"load image points"< raw_worldPts{-3.7500, + 0, + 0.5000, + 7.5000, + 0, + 2.7500, + -3.0000, + -5.0000, + -2.0000, + 3.0000, + 5.0000, + -2.0000, + 0, + 2.2500, + -0.7500, + 0, + -2.2500, + -0.7500}; - std::vector raw_worldPts{ -3.7500, - 0, - 0.5000, - 7.5000, - 0, - 2.7500, - -3.0000, - -5.0000, - -2.0000, - 3.0000, - 5.0000, - -2.0000, - 0, - 2.2500, - -0.7500, - 0, - -2.2500, - -0.7500 - }; + std::cout << "load world points" << std::endl; + for (uint i = 0u; i < raw_worldPts.size(); i += 3) + { + worldPts.push_back(bloody::point3d_type{raw_worldPts[i], raw_worldPts[i + 1], raw_worldPts[i + 2]}); + } - std::cout<<"load world points"<(*maybe_pose); - std::cout< +#include + +namespace bloody +{ + + using stat_info = Eigen::Matrix; + std::tuple, std::vector> + maxPosRatio(Eigen::MatrixXd assignMat); + Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M); + int numMatches(Eigen::MatrixXd assignMat); + + std::pair, bool> softposit( + const std::vector &imagePts, + const std::vector &worldPts, + const Param_type ¶m, + const Pose_type &initpose, + std::pair maybe_caminfo) + { -namespace bloody{ + using namespace std; - using stat_info = arma::vec5; - using stat_info_eigen = Eigen::Matrix; -// std::tuple maxPosRatio(arma::mat assignMat); - std::tuple, Eigen::MatrixXd> - maxPosRatio(Eigen::MatrixXd assignMat); - arma::mat sinkhornSlack(arma::mat M); - arma::mat sinkhornSlack_eigen(arma::mat M); - Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M); - int numMatches(Eigen::MatrixXd assignMat); + std::vector stats; - boost::optional< std::tuple > softposit( - const std::vector& imagePts, - const std::vector& worldPts, - const Param_type& param, - const Pose_type& initpose, - boost::optional maybe_caminfo) - { + auto alpha = 9.21 * std::pow(param.noiseStd, 2) + 1; + auto maxDelta = std::sqrt(alpha) / 2; // 每个世界坐标所允许的最大误差 - using namespace std; + auto betaFinal = 0.5; // 最终迭代目标 + auto betaUpdate = 1.05; // 迭代率 + auto epsilon0 = 0.01; // 用于初始化任务矩阵 - std::vector stats; + auto maxCount = 1; + auto minBetaCount = 20; + auto nbImagePts = imagePts.size(); + auto nbWorldPts = worldPts.size(); - auto alpha = 9.21 * std::pow(param.noiseStd, 2) + 1; - auto maxDelta = std::sqrt(alpha) / 2; // 每个世界坐标所允许的最大误差 + auto minNbPts = std::min(nbImagePts, nbWorldPts); + auto maxNbPts = nbImagePts + nbWorldPts - minNbPts; - auto betaFinal = 0.5; // 最终迭代目标 - auto betaUpdate = 1.05; // 迭代率 - auto epsilon0 = 0.01; // 用于初始化任务矩阵 + auto scale = 1.0 / (maxNbPts + 1); - auto maxCount = 1; - auto minBetaCount = 20; - auto nbImagePts = imagePts.size(); - auto nbWorldPts = worldPts.size(); + std::vector _centeredImage(imagePts.size()); - auto minNbPts = std::min(nbImagePts, nbWorldPts); - auto maxNbPts = nbImagePts + nbWorldPts - minNbPts; + CamInfo_type caminfo; + if (maybe_caminfo.second) + caminfo = maybe_caminfo.first; + else + { + caminfo.focalLength = 1; + caminfo.center = point2di_type{0, 0}; + } - auto scale = 1.0 / (maxNbPts + 1); + std::cout << "init: " << std::endl + << initpose.rot << std::endl + << initpose.trans << std::endl; + Eigen::Matrix centeredImage; + centeredImage.resize(imagePts.size(), Eigen::NoChange); - std::vector _centeredImage(imagePts.size()); + for (size_t i = 0; i < imagePts.size(); ++i) + { + centeredImage.block<1, 2>(i, 0) << (imagePts[i].x() - (double)caminfo.center.x()) / caminfo.focalLength, + (imagePts[i].y() - (double)caminfo.center.y()) / caminfo.focalLength; + } - CamInfo_type caminfo; - if (maybe_caminfo) - caminfo = *maybe_caminfo; - else{ - caminfo.focalLength = 1; - caminfo.center = point2di_type_eigen{0, 0}; - } + std::cout << "centered image :" << centeredImage << std::endl; - std::cout<<"init: "< centeredImage; - centeredImage.resize(imagePts.size(), Eigen::NoChange); + // 齐次坐标 + Eigen::Matrix homogeneousWorldPts; + homogeneousWorldPts.resize(worldPts.size(), Eigen::NoChange); - for (size_t i = 0; i < imagePts.size(); ++i) - { - centeredImage.block<1, 2>(i, 0) << (imagePts[i].x() - (double)caminfo.center.x()) / caminfo.focalLength, - (imagePts[i].y() - (double)caminfo.center.y()) / caminfo.focalLength; - } + for (int i = 0; i < worldPts.size(); ++i) + { + homogeneousWorldPts.block<1, 4>(i, 0) << worldPts[i][0], worldPts[i][1], worldPts[i][2], 1; + } + std::cout << "begin to make world point homogeneous:" << homogeneousWorldPts << std::endl; + + auto pose = initpose; + Eigen::VectorXd wk = homogeneousWorldPts * Eigen::Vector4d{pose.rot(2, 0) / pose.trans[2], pose.rot(2, 1) / pose.trans[2], pose.rot(2, 2) / pose.trans[2], 1}; + std::cout << "wk" << wk << std::endl; + + Eigen::Vector4d r1T = {pose.rot(0, 0) / pose.trans(2), pose.rot(0, 1) / pose.trans(2), pose.rot(0, 2) / pose.trans(2), pose.trans(0) / pose.trans(2)}; + Eigen::Vector4d r2T = {pose.rot(1, 0) / pose.trans(2), pose.rot(1, 1) / pose.trans(2), pose.rot(1, 2) / pose.trans(2), pose.trans(1) / pose.trans(2)}; + + auto betaCount = 0; + auto poseConverged = 0; + auto assignConverged = false; + auto foundPose = 0; + auto beta = param.beta0; + + Eigen::MatrixXd assignMat = Eigen::MatrixXd::Ones(nbImagePts + 1, nbWorldPts + 1) * (1 + epsilon0); + + Eigen::VectorXd imageOnes = Eigen::MatrixXd::Ones(nbImagePts, 1); + + int debug_loop = 0; + while (beta < betaFinal && !assignConverged) + { + std::cout << "debug loop: " << debug_loop++ << std::endl; + + Eigen::VectorXd projectedU = homogeneousWorldPts * r1T; + Eigen::VectorXd projectedV = homogeneousWorldPts * r2T; + + Eigen::MatrixXd replicatedProjectedU = imageOnes * projectedU.transpose(); + Eigen::MatrixXd replicatedProjectedV = imageOnes * projectedV.transpose(); + + std::cout << "r1T, r2T used:" << std::endl + << r1T << std::endl + << r2T << std::endl; + std::cout << "projected uv:" << std::endl + << projectedU << std::endl + << projectedV << std::endl; + std::cout << "reprojected uv" << std::endl; + std::cout << Eigen::MatrixXd(replicatedProjectedU) << std::endl + << Eigen::MatrixXd(replicatedProjectedV) << std::endl; + + std::cout << "SOP" << std::endl; + auto wkxj = centeredImage.col(0) * wk.transpose(); + auto wkyj = centeredImage.col(1) * wk.transpose(); + + std::cout << "wkxj, wkyj" << std::endl; + std::cout << wkxj << std::endl + << wkyj << std::endl; + + Eigen::MatrixXd distMat = caminfo.focalLength * caminfo.focalLength * + ((replicatedProjectedU - wkxj).array().square() + + (replicatedProjectedV - wkyj).array().square()); + + std::cout << "dist mat:" << std::endl + << distMat << std::endl; + + assignMat.block(0, 0, nbImagePts, nbWorldPts) = scale * (-beta * (distMat - alpha * Eigen::MatrixXd::Ones(distMat.rows(), distMat.cols()))).array().exp(); + assignMat.col(nbWorldPts) = scale * Eigen::VectorXd::Ones(nbImagePts + 1); + assignMat.row(nbImagePts) = scale * Eigen::VectorXd::Ones(nbWorldPts + 1).transpose(); + std::cout << "assign befor sinkhorn:" << std::endl + << assignMat << std::endl; + + assignMat = sinkhornImp(assignMat); // My "improved" Sinkhorn. + //assignMat = sinkhornSlack (assignMat); // My "improved" Sinkhorn. + std::cout << "after sinkhorn Slack:" << std::endl + << assignMat << std::endl; + + auto numMatchPts = numMatches(assignMat); + std::cout << "num matches: " << numMatchPts << std::endl; + + auto sumNonslack = assignMat.block(0, 0, nbImagePts, nbWorldPts).sum(); + std::cout << "sum non slack: " << sumNonslack << std::endl; + + Eigen::VectorXd summedByColAssign = assignMat.block(0, 0, nbImagePts, nbWorldPts).colwise().sum(); + Eigen::Matrix4d sumSkSkT = Eigen::Matrix4d::Zero(4, 4); - std::cout<<"centered image :"< homogeneousWorldPts; - homogeneousWorldPts.resize(worldPts.size(), Eigen::NoChange); + for (auto k = 0; k < nbWorldPts; ++k) + { + sumSkSkT = sumSkSkT + summedByColAssign(k) * homogeneousWorldPts.row(k).transpose() * homogeneousWorldPts.row(k); + } + + std::cout << "check ill-condition" << std::endl; + Eigen::JacobiSVD svd_sumSKSkT(sumSkSkT); + double cond = svd_sumSKSkT.singularValues()(0) / svd_sumSKSkT.singularValues()(svd_sumSKSkT.singularValues().size() - 1); + if (cond > 1e10) + { + std::cout << "sumSkSkT is ill-conditioned, termininating search." << std::endl; + return std::make_pair(std::make_tuple(pose, match_type()), false); + } + + Eigen::Matrix4d objectMat = sumSkSkT.inverse(); // Inv(L), a 4x4 matrix. + poseConverged = 0; // Initialize for POSIT loop. + auto pose_iter_count = 0; + + // Save the previouse pose vectors for convergence checks. + Eigen::Vector4d r1Tprev = r1T; + Eigen::Vector4d r2Tprev = r2T; + + double Tx, Ty, Tz; + Eigen::Vector3d r1, r2, r3; + double delta; + + std: + cout << "begin converge loop" << std::endl; + while (poseConverged == false & pose_iter_count < maxCount) + { + + Eigen::Vector4d weightedUi = Eigen::Vector4d::Zero(4); + Eigen::Vector4d weightedVi = Eigen::Vector4d::Zero(4); + + for (int j = 0; j < nbImagePts; ++j) + { + for (int k = 0; k < nbWorldPts; ++k) + { + weightedUi = weightedUi + assignMat(j, k) * wk(k) * centeredImage(j, 0) * homogeneousWorldPts.row(k).transpose(); + weightedVi = weightedVi + assignMat(j, k) * wk(k) * centeredImage(j, 1) * homogeneousWorldPts.row(k).transpose(); + } + } + + r1T = objectMat * weightedUi; + r2T = objectMat * weightedVi; + + Eigen::Vector2d s; + Eigen::Matrix X; + + X.col(0) = r1T.head(3); + X.col(1) = r2T.head(3); + + std::cout << "svd" << std::endl; + Eigen::JacobiSVD> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix II; + II << 1, 0, 0, 1, 0, 0; + s = svd.singularValues(); + Eigen::MatrixXd A = svd.matrixU() * II * svd.matrixV().transpose(); + + r1 = A.col(0); + r2 = A.col(1); + r3 = r1.cross(r2); + + Tz = 2 / (s(0) + s(1)); + Tx = r1T(3) * Tz; + Ty = r2T(3) * Tz; + auto r3T = Eigen::Vector4d{r3[0], r3[1], r3[2], Tz}; + + std::cout << "svd" << std::endl + << A << r1 << r2 << r3 << Tx << Ty << Tz << std::endl; + + r1T = Eigen::Vector4d{r1[0], r1[1], r1[2], Tx} / Tz; + r2T = Eigen::Vector4d{r2[0], r2[1], r2[2], Ty} / Tz; + std::cout << "r1T, r2T update: " << r1T << std::endl + << r2T << std::endl; + + wk = homogeneousWorldPts * r3T / Tz; + + std::cout << "delta" << std::endl; + delta = std::sqrt((assignMat.block(0, 0, nbImagePts, nbWorldPts).cwiseProduct(distMat) / (double)nbWorldPts).sum()); + poseConverged = delta < maxDelta; + + std::cout << "pose converged:" << poseConverged << std::endl; + + std::cout << "generate trace" << std::endl; + + stat_info trace; + trace << beta, delta, double(numMatchPts) / nbWorldPts, + double(sumNonslack) / nbWorldPts, + ((r1T - r1Tprev).array().square() + (r2T - r2Tprev).array().square()).sum(); + + std::cout << "keep log" << std::endl; + stats.push_back(trace); + + pose_iter_count = pose_iter_count + 1; + } + + beta = betaUpdate * beta; + betaCount = betaCount + 1; + assignConverged = poseConverged && betaCount > minBetaCount; + + pose.trans = Eigen::Vector3d{Tx, Ty, Tz}; + pose.rot.row(0) = r1.transpose(); + pose.rot.row(1) = r2.transpose(); + pose.rot.row(2) = r3.transpose(); + + foundPose = (delta < maxDelta && betaCount > minBetaCount); + + std::cout << "updated pose:" << std::endl + << pose.rot << std::endl + << pose.trans << std::endl; + //%% Log: + std::cout << "converge loop ends" << std::endl; + std::cout << "pose found: " << foundPose + << "delta exit: " << (delta < maxDelta) + << ", count exit:" << (betaCount > minBetaCount) << std::endl; + } - for (int i = 0; i < worldPts.size(); ++i) - { - homogeneousWorldPts.block<1, 4>(i, 0) << worldPts[i][0], worldPts[i][1], worldPts[i][2],1; + std::cout << "pose converged:" << std::endl + << pose.rot << pose.trans << std::endl; + return std::make_pair(std::make_tuple(pose, match_type()), true); } - std::cout<<"begin to make world point homogeneous:" << homogeneousWorldPts < svd_sumSKSkT(sumSkSkT); - double cond = svd_sumSKSkT.singularValues()(0) / svd_sumSKSkT.singularValues()(svd_sumSKSkT.singularValues().size()-1); - if (cond > 1e10){ - std::cout<<"sumSkSkT is ill-conditioned, termininating search."< other_cols; + other_cols.reserve(assignMat.cols() - 1); + for (int i = 0; i < assignMat.cols(); ++i) + if (i != k) + other_cols.push_back(i); + unsigned long long kmax; + assignMat.row(imax).maxCoeff(&kmax); + if (kmax == k) + { + num = num + 1; // This value is maximal in its row & column. + } } - - r1T= objectMat * weightedUi; - r2T = objectMat * weightedVi; - - arma::mat U, V; - Eigen::Vector2d s; - Eigen::Matrix X; - - X.col(0) = r1T.head(3); - X.col(1) = r2T.head(3); - - std::cout<<"svd"<> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV); - - // arma::svd(U,s,V, X); - Eigen::Matrix II; - II << 1, 0, 0, 1, 0, 0 ; - s = svd.singularValues(); - Eigen::MatrixXd A = svd.matrixU() * II * svd.matrixV().transpose(); - - r1 = A.col(0); - r2 = A.col(1); - r3 = r1.cross(r2); - - Tz = 2 / (s(0) + s(1)); - Tx = r1T(3) * Tz; - Ty = r2T(3) * Tz; - auto r3T= Eigen::Vector4d{r3[0], r3[1], r3[2], Tz}; - - std::cout<<"svd"<{ - beta ,delta ,double(numMatchPts)/nbWorldPts , - double(sumNonslack)/nbWorldPts, - ((r1T-r1Tprev).array().square() + (r2T-r2Tprev).array().square()).sum() - }; - - std::cout<<"keep log"< minBetaCount; - - pose.trans = Eigen::Vector3d{Tx, Ty, Tz}; - pose.rot.row(0) = r1.transpose(); - pose.rot.row(1) = r2.transpose(); - pose.rot.row(2) = r3.transpose(); - - foundPose = (delta < maxDelta && betaCount > minBetaCount); - - std::cout<<"updated pose:"<minBetaCount)< fEpsilon2 && iNumSinkIter < iMaxIterSinkhorn){ - auto Mprev = M; // % Save M from previous iteration to test for loop termination - - // Col normalization (except outlier row - do not normalize col slacks against each other) - arma::rowvec McolSums = arma::sum(M); // Row vector. - McolSums(nbCols-1) = 1; // Don't normalize slack col terms against each other. - auto McolSumsRep = arma::ones(nbRows) * McolSums ; - M = M / McolSumsRep; - - // Row normalization (except outlier row - do not normalize col slacks against each other) - arma::colvec MrowSums = arma::sum(M, 1); // Column vector. - MrowSums(nbRows-1) = 1; // Don't normalize slack row terms against each other. - auto MrowSumsRep = MrowSums * arma::ones(nbCols); - M = M / MrowSumsRep; + Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M) + { + Eigen::MatrixXd normalizedMat; + + auto iMaxIterSinkhorn = 60; // In PAMI paper + auto fEpsilon2 = 0.001; // Used in termination of Sinkhorn Loop. + + auto iNumSinkIter = 0; + auto nbRows = M.rows(); + auto nbCols = M.cols(); + + auto fMdiffSum = fEpsilon2 + 1; // Set "difference" from last M matrix above the loop termination threshold + + // Get the positions and ratios to slack of the nonslack elements that are maximal in their row and column. + // Eigen::Matrix + std::vector posmax; + std::vector ratios; + std::tie(posmax, ratios) = maxPosRatio(M); + // std::cout<<"postmax, rations"< fEpsilon2 && iNumSinkIter < iMaxIterSinkhorn) + { + auto Mprev = M; // Save M from previous iteration to test for loop termination + + // Col normalization (except outlier row - do not normalize col slacks + // against each other) + Eigen::VectorXd McolSums = M.colwise().sum(); // Row vector. 按列求和 + McolSums(nbCols - 1) = 1; // Don't normalize slack col terms against each other. + + Eigen::MatrixXd McolSumsRep = Eigen::VectorXd::Ones(nbRows) * McolSums.transpose(); + M = M.cwiseQuotient(McolSumsRep); + + // Fix values in the slack column. + for (auto i = 0; i < posmax.size(); i += 2) + { + M(posmax[i], nbCols - 1) = ratios[i] * M(posmax[i], posmax[i + 1]); + } + // Row normalization (except outlier row - do not normalize col slacks against each other) + Eigen::VectorXd MrowSums = M.rowwise().sum(); // Column vector. + MrowSums(nbRows - 1) = 1; // Don't normalize slack row terms against each other. + + auto MrowSumsRep = MrowSums * Eigen::VectorXd::Ones(nbCols).transpose(); + M = M.cwiseQuotient(MrowSumsRep); + + // Fix values in the slack row. + for (auto i = 0; i < posmax.size(); i += 2) + { + M(nbRows - 1, posmax[i + 1]) = ratios[i + 1] * M(posmax[i], posmax[i + 1]); + } + iNumSinkIter = iNumSinkIter + 1; + fMdiffSum = (M - Mprev).cwiseAbs().sum(); + } + normalizedMat = M; - iNumSinkIter=iNumSinkIter+1; - fMdiffSum=arma::accu(arma::abs(M-Mprev)); + return normalizedMat; } - normalizedMat = M; - - return normalizedMat; - } - - - int numMatches(Eigen::MatrixXd assignMat) - { - int num = 0; - - auto nimgpnts = assignMat.rows() - 1; - auto nmodpnts = assignMat.cols() - 1; - - for (int k = 0; k< nmodpnts; ++k){ - - unsigned long long imax; - auto vmax = assignMat.col(k).maxCoeff(&imax); - - if(imax == assignMat.rows() - 1) continue; // Slack value is maximum in this column. - - std::vector other_cols; - other_cols.reserve(assignMat.cols()-1); - for (int i=0; i, std::vector> + maxPosRatio(Eigen::MatrixXd assignMat) + { + // function [pos, ratios] = maxPosRatio(assignMat) + std::vector pos_; + std::vector ratios_; + + auto nrows = assignMat.rows(); //size(assignMat,1); + auto ncols = assignMat.cols(); //size(assignMat,2); + auto nimgpnts = nrows - 1; + auto nmodpnts = ncols - 1; + // 按列遍历 + for (auto k = 0u; k < nmodpnts; ++k) + { + unsigned long long imax, __t; + auto vmax = assignMat.col(k).maxCoeff(&imax); + + if (imax == nrows - 1) + continue; // Slack value is maximum in this column. + + // 检查列中的最大值在其行中是否为最大值. + // 在其行中是最大值 + unsigned long long kmax; + auto _t = assignMat.row(imax).maxCoeff(&kmax); + if (kmax == k) + { + pos_.push_back(imax); + pos_.push_back(k); + // Compute the ratios to row and column slack values. + auto rr = assignMat(imax, ncols - 1) / assignMat(imax, k); + auto cr = assignMat(nrows - 1, k) / assignMat(imax, k); + ratios_.push_back(rr); + ratios_.push_back(cr); + } + } + return std::make_tuple(pos_, ratios_); } - return num; - } - - - Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M){ - Eigen::MatrixXd normalizedMat; - - auto iMaxIterSinkhorn = 60; // In PAMI paper - auto fEpsilon2 = 0.001; // Used in termination of Sinkhorn Loop. - - auto iNumSinkIter = 0; - auto nbRows = M.rows(); - auto nbCols = M.cols(); - auto fMdiffSum = fEpsilon2 + 1; // Set "difference" from last M matrix above the loop termination threshold + void softposit(float *rot, float *trans, int *foundPose, int *_imagePts, float *_worldPts, int nbImagePts, int nbWorldPts, float beta0, float noiseStd, float *initRot, float *initTrans, float *focalLength, int *center) + { -// Get the positions and ratios to slack of the nonslack elements that are maximal in their row and column. - // Eigen::Matrix - Eigen::Matrix posmax; - Eigen::Matrix ratios; - std::tie(posmax, ratios) = maxPosRatio(M); - std::cout<<"postmax, rations"< imagePts; + std::vector worldPts; - while(fabs(fMdiffSum) > fEpsilon2 && iNumSinkIter < iMaxIterSinkhorn) - { - auto Mprev = M; // Save M from previous iteration to test for loop termination - - // Col normalization (except outlier row - do not normalize col slacks - // against each other) - Eigen::VectorXd McolSums = M.colwise().sum(); // Row vector. 按列求和 - McolSums(nbCols-1) = 1; // Don't normalize slack col terms against each other. - - Eigen::MatrixXd McolSumsRep = Eigen::VectorXd::Ones(nbRows) * McolSums.transpose(); - M = M.cwiseQuotient(McolSumsRep); - - // Fix values in the slack column. - for (auto i=0; i< posmax.rows(); ++i){ - M(posmax(i,0), nbCols-1) = ratios(i,0)*M(posmax(i,0),posmax(i,1)); - } - // Row normalization (except outlier row - do not normalize col slacks against each other) - Eigen::VectorXd MrowSums = M.rowwise().sum(); // Column vector. - MrowSums(nbRows-1) = 1; // Don't normalize slack row terms against each other. - - auto MrowSumsRep = MrowSums * Eigen::VectorXd::Ones(nbCols).transpose(); - M = M.cwiseQuotient(MrowSumsRep); - - // Fix values in the slack row. - for (auto i=0; i< posmax.rows(); ++i){ - M(nbRows-1,posmax(i,1)) = ratios(i,1)*M(posmax(i,0),posmax(i,1)); - } - iNumSinkIter=iNumSinkIter+1; - fMdiffSum=(M-Mprev).cwiseAbs().sum(); + for (uint i = 0u; i < nbImagePts; ++i) + { + imagePts.push_back(point2di_type{_imagePts[i * 2], _imagePts[i * 2 + 1]}); + } + for (uint i = 0u; i < nbWorldPts; ++i) + { + worldPts.push_back(point3d_type{_worldPts[i * 3], _worldPts[3 * i + 1], _worldPts[3 * i + 2]}); + } - } - normalizedMat = M; - - return normalizedMat; - } - - - std::tuple, Eigen::MatrixXd> - maxPosRatio(Eigen::MatrixXd assignMat) - { -// function [pos, ratios] = maxPosRatio(assignMat) - Eigen::Matrix pos; - Eigen::Matrix ratios; - std::vector pos_; - std::vector ratios_; - - auto nrows = assignMat.rows(); //size(assignMat,1); - auto ncols = assignMat.cols(); //size(assignMat,2); - auto nimgpnts = nrows - 1; - auto nmodpnts = ncols - 1; - arma::mat test; - // 按列遍历 - for (auto k=0u; k, Eigen::Unaligned>(pos_.data(), pos_.size()); - ratios = Eigen::Map, Eigen::Unaligned>(ratios_.data(), ratios_.size()); - return std::make_tuple(pos, ratios); - } - - // void softposit(float* rot, float* trans, int* foundPose, int* _imagePts, float* _worldPts, int nbImagePts, int nbWorldPts, float beta0, float noiseStd, float* initRot, float* initTrans, float* focalLength, int* center) - // { - - // std::vector imagePts; - // std::vector worldPts; - - // for (uint i=0u; i #include -#include -#include +#include #include #include #include - -namespace bloody{ -// c warp. - void softposit(float* rot, float* trans, int* foundPose, int* _imagePts, float* _worldPts, int nbImagePts, int nbWorldPts, float beta0, float noiseStd, float* initRot, float* initTrans, float* focalLength, int* center); - -// c++ imp. - using vec3_type = arma::vec3; - using mat3_type = arma::mat33; - - using point2di_type = arma::ivec2; - using point2d_type = arma::vec2; - using point3d_type = arma::vec3; - - using vec3_type_eigen = Eigen::Vector3d; - using mat3_type_eigen = Eigen::Matrix3d; - - using point2di_type_eigen = Eigen::Vector2i; - using point2d_type_eigen = Eigen::Vector3d; - using point3d_type_eigen = Eigen::Vector3d; - - using match_type = std::vector >; - - struct Param_type{ - double beta0; - double noiseStd; - }; - - struct Pose_type{ - mat3_type_eigen rot; - vec3_type_eigen trans; - }; - - struct CamInfo_type{ - double focalLength; - point2di_type_eigen center; - }; - - -// at least it will find something - boost::optional< std::tuple > softposit( - const std::vector& imagePts, - const std::vector& worldPts, - const Param_type& param, - const Pose_type& initpose, - boost::optional maybe_caminfo); - - -} +namespace bloody +{ + // c warp. + void softposit(float *rot, float *trans, int *foundPose, int *_imagePts, float *_worldPts, int nbImagePts, int nbWorldPts, float beta0, float noiseStd, float *initRot, float *initTrans, float *focalLength, int *center); + + // c++ imp. + using vec3_type = Eigen::Vector3d; + using mat3_type = Eigen::Matrix3d; + + using point2di_type = Eigen::Vector2i; + using point2d_type = Eigen::Vector3d; + using point3d_type = Eigen::Vector3d; + + using match_type = std::vector>; + + struct Param_type + { + double beta0; + double noiseStd; + }; + + struct Pose_type + { + mat3_type rot; + vec3_type trans; + }; + + struct CamInfo_type + { + double focalLength; + point2di_type center; + }; + + // at least it will find something + std::pair, bool> softposit( + const std::vector &imagePts, + const std::vector &worldPts, + const Param_type ¶m, + const Pose_type &initpose, + std::pair maybe_caminfo); + +} // namespace bloody #endif /* uu_recreate_wheel_imp_SOFTPOSIT_H */ From d3da5ac333d0b15f527663e6ba41c7790d384fbb Mon Sep 17 00:00:00 2001 From: wqhot Date: Tue, 13 Oct 2020 15:30:55 +0800 Subject: [PATCH 4/4] update readme --- README.md | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index da7f84c..706cf53 100644 --- a/README.md +++ b/README.md @@ -21,13 +21,10 @@ This repo. provides my cpp implementation. # How to Build ## Dependency -1. Armadillo +1. Eigen3 -It mainly depends on the Armadillo linear algebra library. +It mainly depends on the Eigen3 linear algebra library. -2. Boost (Format) - -There are also dependency on Boost, and only for outside logging code which can be easily removed. ## Build Steps The project is managed with CMake. @@ -35,8 +32,9 @@ The project is managed with CMake. Suggested steps: ``` -git clone git@github.com:autosquid/softposit.git +git clone https://github.com/wqhot/softposit-cpp cd softposit +git checkout removearma mkdir build cd build cmake ..