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/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/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/.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 2e4eb81..8010f02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,11 +8,9 @@ IF(CMAKE_VERSION VERSION_EQUAL "3.0.0" OR CMAKE_VERSION VERSION_GREATER "3.0.0") 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 +endif() set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}) set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}) @@ -40,23 +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/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 .. diff --git a/src/demo/demo.cpp b/src/demo/demo.cpp index bd232e8..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; - std::tuple maxPosRatio(arma::mat assignMat); - arma::mat sinkhornSlack(arma::mat M); - arma::mat sinkhornImp(arma::mat M); - int numMatches(arma::mat 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*pow(param.noiseStd,2) + 1; - auto maxDelta = sqrt(alpha)/2; // Max allowed error per world point. + auto minNbPts = std::min(nbImagePts, nbWorldPts); + auto maxNbPts = nbImagePts + nbWorldPts - minNbPts; - 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 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{0, 0}; - } + std::cout << "centered image :" << centeredImage << std::endl; - std::cout<<"init: "< homogeneousWorldPts; + homogeneousWorldPts.resize(worldPts.size(), Eigen::NoChange); - std::transform(imagePts.begin(), imagePts.end(), - _centeredImage.begin(), - [&caminfo](const point2di_type & _pt){ - return point2d_type((point2d_type{double(_pt[0]), double(_pt[1])} - caminfo.center)/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); - arma::mat centeredImage = arma::zeros(_centeredImage.size(),2); + 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 j=0; j(worldPts.size(), 4).eval(); - for (int i=0; i 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. + } + } + return num; } - std::cout<<"begin to make world point homogeneous:" << homogeneousWorldPts <(nbImagePts, 1); - - int debug_loop = 0; - while (beta < betaFinal && !assignConverged) + Eigen::MatrixXd sinkhornImp(Eigen::MatrixXd M) { - std::cout<(nbImagePts+1); - assignMat.row(nbImagePts) = scale * arma::ones(nbWorldPts+1); - std::cout<<"assign befor sinkhorn:"<(4, 4); - - for(auto k = 0; k 1e10){ - std::cout<<"sumSkSkT is ill-conditioned, termininating search."< + 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; - r1T= objectMat * weightedUi; - r2T = objectMat * weightedVi; - - arma::mat U, V; - arma::vec s; - arma::mat X(3,2); - - X.col(0) = r1T(arma::span(0,2)); - X.col(1) = r2T(arma::span(0,2)); - - std::cout<<"svd"<{ - beta ,delta ,double(numMatchPts)/nbWorldPts , - double(sumNonslack)/nbWorldPts, - arma::accu(arma::square(r1T-r1Tprev)) + arma::accu(arma::square(r2T-r2Tprev)) - }; - - 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(); - - 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; - - iNumSinkIter=iNumSinkIter+1; - fMdiffSum=arma::accu(arma::abs(M-Mprev)); + return normalizedMat; } - normalizedMat = M; - - return normalizedMat; - } - - - int numMatches(arma::mat assignMat) - { - int num = 0; - - auto nimgpnts = assignMat.n_rows - 1; - auto nmodpnts = assignMat.n_cols - 1; - - for (int k = 0; k< nmodpnts; ++k){ - - arma::uword imax; - auto vmax = assignMat.col(k).max(imax); - - if(imax == assignMat.n_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 assignMat.submat(arma::uvec{imax},arma::uvec(other_cols))))) - num = num + 1; // This value is maximal in its row & column. - } - return num; - } - - - arma::mat sinkhornImp(arma::mat M){ - arma::mat normalizedMat; - - auto iMaxIterSinkhorn=60; // In PAMI paper - auto fEpsilon2 = 0.001; // Used in termination of Sinkhorn Loop. - - auto iNumSinkIter = 0; - auto nbRows = M.n_rows; - auto nbCols = M.n_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. - arma::umat posmax; - arma::mat ratios; - std::tie(posmax, ratios) = maxPosRatio(M); - std::cout<<"postmax, rations"< fEpsilon2 && iNumSinkIter < iMaxIterSinkhorn) + std::tuple, std::vector> + maxPosRatio(Eigen::MatrixXd assignMat) { - 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, 0); // Row vector. - McolSums(nbCols-1) = 1; // Don't normalize slack col terms against each other. - - auto McolSumsRep = arma::ones(nbRows) * McolSums ; - M = M / McolSumsRep; - - // Fix values in the slack column. - for (auto i=0; i< posmax.n_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. - MrowSums(nbRows-1) = 1; // Don't normalize slack row terms against each other. - - auto MrowSumsRep = MrowSums * arma::ones(nbCols); - M = M / MrowSumsRep; - - // Fix values in the slack row. - for (auto i=0; i< posmax.n_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)); - - } - normalizedMat = M; - - return normalizedMat; - } - - - std::tuple maxPosRatio(arma::mat assignMat) - { -// function [pos, ratios] = maxPosRatio(assignMat) - arma::umat pos; - arma::mat ratios; - - auto nrows = assignMat.n_rows; //size(assignMat,1); - auto ncols = assignMat.n_cols; //size(assignMat,2); - auto nimgpnts = nrows - 1; - auto nmodpnts = ncols - 1; - - 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})); - - // 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})); - } + // 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 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) + { - 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; - std::vector imagePts; - std::vector worldPts; + 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]}); + } - for (uint i=0u; i -#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 match_type = std::vector >; - - struct Param_type{ - double beta0; - double noiseStd; - }; - - struct Pose_type{ - mat3_type rot; - vec3_type trans; - }; - - struct CamInfo_type{ - float focalLength; - point2di_type 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 */