TEASER++是《TEASER：Fast and Certifiable Point Cloud Registration》提出的快速且可证明的点云配准算法。可以在毫秒级别完成大型点云的配准。
原仓库维护的很好，但是只支持linux而不支持windows。强烈推荐在linux上使用teaserpp。但是笔者已经有一部分相关工作在windows上了，因此希望能够在windows上复现teaser的结果。在官方的文档中提到了WinTeaser这个库，但是这个库似乎对Teaser++代码作出了某些修改（?)因此还是希望能够在官方库代码的基础上适配windows。
笔者的环境为
- Visual Studio 2015
- MSVC 19.40.33813
- PCL 1.8.1
- Eigen 3.4.0
pcl的属性表配置不再赘述，可以自己在网络上搜索。 先尝试一下用cmake构建teaser++。根据指引configre generate 生成vs项目，然后使用VS build。报错找不到<sys/time.h> <dirent.h> 。这是unix系统的头文件，windows并不支持。经过笔者一番考证，teaserpp的本体中并没有使用到这两个头文件。问题出在第三方库 pmc 上。pmc不支持windows，因而teaserpp在windows上生成会报错。但是话又说回来，<sys/time.h>和<dirent.h>本身也不是特别的复杂的库，不涉及核心的计算功能。如果将用到这部分代码的文件重构为winapi实现也许就能在windwos上运行teaser了呢！
新建一个vs项目，添加pcl props。将github.com/MIT-SPARK/T… 目录下teaser的核心代码添加到项目中。将build目录下的spectra(TEASER-plusplus\build\spectra-src\include)添加到头文件目录。pmc(TEASER-plusplus\build\pmc-src\include)添加到头文件目录。TEASER-plusplus\build\pmc-src下的cpp文件添加到项目源代码。vs项目的结构大概长这样
运行后发现，同样是提示找不到文件。于是我们删除所有对<sys/time.h> <dirent.h>的include，使用winapi重构pmc的这部分代码 这两个头文件应该只在pmc_utils.cpp中应用了，重构一下pmc_utils.cpp pmc_utils.cpp
#include <windows.h>
#include <psapi.h>
#include <direct.h>
#include <io.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cassert>
#include <set>
#include <fstream>
#include <cstdio>
#include <pmc/pmc_utils.h>
using namespace std;
bool fexists(const char* filename) {
return (_access(filename, 0) != -1);
}
void usage(char* argv0) {
const char* params =
"Usage: %s -a alg -f graphfile ... (same as before) \n";
fprintf(stderr, params, argv0);
exit(-1);
}
double get_time() {
static LARGE_INTEGER freq;
static BOOL initialized = QueryPerformanceFrequency(&freq);
LARGE_INTEGER now;
QueryPerformanceCounter(&now);
return static_cast<double>(now.QuadPart) / freq.QuadPart;
}
string memory_usage() {
PROCESS_MEMORY_COUNTERS pmc;
if (GetProcessMemoryInfo(GetCurrentProcess(), &pmc, sizeof(pmc))) {
ostringstream mem;
mem << "Memory Usage (VmSize): " << pmc.WorkingSetSize / 1024 << " KB";
return mem.str();
}
return "Unable to get memory usage.";
}
void indent(int level, string str) {
for (int i = 0; i < level; i++)
cout << " ";
cout << "(" << level << ") ";
}
void print_max_clique(vector<int>& C) {
#ifdef PMC_ENABLE_DEBUG
cout << "Maximum clique: ";
for (int i = 0; i < C.size(); i++)
cout << C[i] + 1 << " ";
cout << endl;
#endif
}
void print_n_maxcliques(set<vector<int>> C, int n) {
#ifdef PMC_ENABLE_DEBUG
int mc = 0;
for (auto it = C.begin(); it != C.end() && mc < n; ++it, ++mc) {
cout << "Maximum clique: ";
for (int j = 0; j < it->size(); j++)
cout << (*it)[j] << " ";
cout << endl;
}
#endif
}
void validate(bool condition, const string& msg) {
if (!condition) {
cerr << msg << endl;
assert(condition);
}
}
int getdir(string dir, vector<string>& files) {
string search_path = dir + "\\*";
WIN32_FIND_DATAA fd;
HANDLE hFind = FindFirstFileA(search_path.c_str(), &fd);
if (hFind == INVALID_HANDLE_VALUE) {
cout << "Error opening directory: " << dir << endl;
return -1;
}
do {
if (strcmp(fd.cFileName, ".") != 0 && strcmp(fd.cFileName, "..") != 0)
files.push_back(fd.cFileName);
} while (FindNextFileA(hFind, &fd));
FindClose(hFind);
return 0;
}
此外，因为有pcl了，也不需要tinyply了。所以项目里没tinyply。重构了teaser/ply_io.cc
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "teaser/ply_io.h"
int teaser::PLYReader::read(const std::string& file_name, teaser::PointCloud& cloud) {
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
if (pcl::io::loadPLYFile<pcl::PointXYZ>(file_name, pcl_cloud) < 0) {
std::cerr << "Failed to read PLY file: " << file_name << std::endl;
return -1;
}
cloud.clear();
cloud.reserve(pcl_cloud.size());
for (const auto& pt : pcl_cloud.points) {
cloud.push_back({ pt.x, pt.y, pt.z });
}
return 0;
}
int teaser::PLYWriter::write(const std::string& file_name, const teaser::PointCloud& cloud,
bool binary_mode) {
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl_cloud.reserve(cloud.size());
for (const auto& pt : cloud) {
pcl_cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
pcl_cloud.width = pcl_cloud.size();
pcl_cloud.height = 1;
pcl_cloud.is_dense = true;
if (binary_mode) {
if (pcl::io::savePLYFileBinary(file_name, pcl_cloud) < 0) {
std::cerr << "Failed to write binary PLY file: " << file_name << std::endl;
return -1;
}
}
else {
if (pcl::io::savePLYFileASCII(file_name, pcl_cloud) < 0) {
std::cerr << "Failed to write ASCII PLY file: " << file_name << std::endl;
return -1;
}
}
return 0;
}
把这两部分改完之后编译其实还遇到了一个问题，就是pmc_input.h中input的构造函数问题。input(int argc, char* argv[])构造函数中用到了getopt()函数，windows不支持。查找引用，发现只有pmc_driver.cpp中用到了这一构造函数。所以解决方法简单粗暴：删掉input(int argc, char* argv[])，只保留input()构造。从项目中移除pmc_driver.cpp(需要注意的是，这个cpp里有main函数，即使不修改input也应该删除)。还是贴一下修改后的pmc_input.h
/**
============================================================================
Name : Parallel Maximum Clique (PMC) Library
Author : Ryan A. Rossi (rrossi@purdue.edu)
Description : A general high-performance parallel framework for computing
maximum cliques. The library is designed to be fast for large
sparse graphs.
Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved.
Please cite the following paper if used:
Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa
Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs
and Temporal Strong Components, arXiv preprint 1302.6256, 2013.
See http://ryanrossi.com/pmc for more information.
============================================================================
*/
#ifndef PMC_INPUT_H_
#define PMC_INPUT_H_
#include "pmc_headers.h"
#include "pmc_utils.h"
using namespace std;
namespace pmc {
class input {
public:
// instance variables
int algorithm;
int threads;
int experiment;
int lb;
int ub;
int param_ub;
int adj_limit;
double time_limit;
double remove_time;
bool graph_stats;
bool verbose;
bool help;
bool MCE;
bool decreasing_order;
string heu_strat;
string format;
string graph;
string output;
string edge_sorter;
string vertex_search_order;
input() {
// default values
algorithm = 0;
threads = omp_get_max_threads();
experiment = 0;
lb = 0;
ub = 0;
param_ub = 0;
adj_limit = 20000;
time_limit = 60 * 60; // max time to search
remove_time = 4.0; // time to wait before reducing graph
verbose = false;
graph_stats = false;
help = false;
MCE = false;
decreasing_order = false;
heu_strat = "kcore";
vertex_search_order = "deg";
format = "mtx";
graph = "data/sample.mtx";
output = "";
string edge_sorter = "";
// both off, use default alg
if (heu_strat == "0" && algorithm == -1)
algorithm = 0;
if (threads <= 0) threads = 1;
}
/*
input(int argc, char* argv[]) {
// default values
algorithm = 0;
threads = omp_get_max_threads();
experiment = 0;
lb = 0;
ub = 0;
param_ub = 0;
adj_limit = 20000;
time_limit = 60 * 60; // max time to search
remove_time = 4.0; // time to wait before reducing graph
verbose = false;
graph_stats = false;
help = false;
MCE = false;
decreasing_order = false;
heu_strat = "kcore";
vertex_search_order = "deg";
format = "mtx";
graph = "data/sample.mtx";
output = "";
string edge_sorter = "";
int opt;
while ((opt = getopt(argc, argv, "i:t:f:u:l:o:e:a:r:w:h:k:dgsv")) != EOF) {
switch (opt) {
case 'a':
algorithm = atoi(optarg);
if (algorithm > 9) MCE = true;
break;
case 't':
threads = atoi(optarg);
break;
case 'f':
graph = optarg;
break;
case 's':
graph_stats = true;
break;
case 'u':
param_ub = atoi(optarg); // find k-clique fast
lb = 2; // skip heuristic
break;
case 'k':
param_ub = atoi(optarg);
lb = param_ub - 1;
break;
case 'l':
lb = atoi(optarg);
break;
case 'h':
heu_strat = optarg;
break;
case 'v':
verbose = 1;
break;
case 'w':
time_limit = atof(optarg) * 60; // convert minutes to seconds
break;
case 'r':
remove_time = atof(optarg);
break;
case 'e':
edge_sorter = optarg;
break;
case 'o':
vertex_search_order = optarg;
break;
case 'd':
// direction of which vertices are ordered
decreasing_order = true;
break;
case '?':
usage(argv[0]);
break;
default:
usage(argv[0]);
break;
}
}
// both off, use default alg
if (heu_strat == "0" && algorithm == -1)
algorithm = 0;
if (threads <= 0) threads = 1;
if (!fexists(graph.c_str())) {
usage(argv[0]);
exit(-1);
}
FILE* fin = fopen(graph.c_str(), "r+t");
if (fin == NULL) {
usage(argv[0]);
exit(-1);
}
fclose(fin);
cout << "\n\nFile Name ------------------------ " << graph.c_str() << endl;
if (!fexists(graph.c_str())) {
cout << "File not found!" << endl;
return;
}
cout << "workers: " << threads << endl;
omp_set_num_threads(threads);
}
*/
};
} // namespace pmc
#endif
然后就能快乐的编译了！然后就链接报错了
无法解析的外部符号 LZ4_compress_HC_continue
问了一下chatgpt，lz4是一个压缩库。（可能是FLANN pcl 或者teaserpp在用？笔者没有深究，写这篇文章的时候楼主回忆也有可能是重构的ply_io.cc的某个地方用到了）。 最方便的方法肯定还是直接去找一个lz4.lib链接到项目。神奇的是，笔者用everything在matlab中找到了一个lz4.lib.....(D:\MATLAB\sys\ros1\win64\ros1\lz4\lib)如果没找到的朋友可以去参考网络上的文章编译lz4。了解原因的朋友也可以评论指出一下。
链接lz4.lib后，生成成功！可以的在windows上运行teaser啦 再贴一下测试代码main.cpp
// An example showing TEASER++ registration with the Stanford bunny model
#include <chrono>
#include <iostream>
#include <random>
#include <Eigen/Core>
#include <teaser/ply_io.h>
#include <teaser/registration.h>
// Macro constants for generating noise and outliers
#define NOISE_BOUND 0.001
#define N_OUTLIERS 1700
#define OUTLIER_TRANSLATION_LB 5
#define OUTLIER_TRANSLATION_UB 10
inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) {
return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0)));
}
void addNoiseAndOutliers(Eigen::Matrix<double, 3, Eigen::Dynamic>& tgt) {
// Add uniform noise
Eigen::Matrix<double, 3, Eigen::Dynamic> noise =
Eigen::Matrix<double, 3, Eigen::Dynamic>::Random(3, tgt.cols())* NOISE_BOUND / 2;
tgt = tgt + noise;
// Add outliers
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers
std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB,
OUTLIER_TRANSLATION_UB); // random translation
std::vector<bool> expected_outlier_mask(tgt.cols(), false);
for (int i = 0; i < N_OUTLIERS; ++i) {
int c_outlier_idx = dis2(gen);
assert(c_outlier_idx < expected_outlier_mask.size());
expected_outlier_mask[c_outlier_idx] = true;
tgt.col(c_outlier_idx).array() += dis3(gen); // random translation
}
}
int main() {
// Load the .ply file
teaser::PLYReader reader;
teaser::PointCloud src_cloud;
auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud);
int N = src_cloud.size();
// Convert the point cloud to Eigen
Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, N);
for (size_t i = 0; i < N; ++i) {
src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z;
}
// Homogeneous coordinates
Eigen::Matrix<double, 4, Eigen::Dynamic> src_h;
src_h.resize(4, src.cols());
src_h.topRows(3) = src;
src_h.bottomRows(1) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(N);
// Apply an arbitrary SE(3) transformation
Eigen::Matrix4d T;
// clang-format off
T << 9.96926560e-01, 6.68735757e-02, -4.06664421e-02, -1.15576939e-01,
-6.61289946e-02, 9.97617877e-01, 1.94008687e-02, -3.87705398e-02,
4.18675510e-02, -1.66517807e-02, 9.98977765e-01, 1.14874890e-01,
0, 0, 0, 1;
// clang-format on
// Apply transformation
Eigen::Matrix<double, 4, Eigen::Dynamic> tgt_h = T * src_h;
Eigen::Matrix<double, 3, Eigen::Dynamic> tgt = tgt_h.topRows(3);
// Add some noise & outliers
addNoiseAndOutliers(tgt);
// Run TEASER++ registration
// Prepare solver parameters
teaser::RobustRegistrationSolver::Params params;
params.noise_bound = NOISE_BOUND;
params.cbar2 = 1;
params.estimate_scaling = false;
params.rotation_max_iterations = 100;
params.rotation_gnc_factor = 1.4;
params.rotation_estimation_algorithm =
teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
params.rotation_cost_threshold = 0.005;
// Solve with TEASER++
teaser::RobustRegistrationSolver solver(params);
std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
solver.solve(src, tgt);
std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
auto solution = solver.getSolution();
// Compare results
std::cout << "=====================================" << std::endl;
std::cout << " TEASER++ Results " << std::endl;
std::cout << "=====================================" << std::endl;
std::cout << "Expected rotation: " << std::endl;
std::cout << T.topLeftCorner(3, 3) << std::endl;
std::cout << "Estimated rotation: " << std::endl;
std::cout << solution.rotation << std::endl;
std::cout << "Error (rad): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation)
<< std::endl;
std::cout << std::endl;
std::cout << "Expected translation: " << std::endl;
std::cout << T.topRightCorner(3, 1) << std::endl;
std::cout << "Estimated translation: " << std::endl;
std::cout << solution.translation << std::endl;
std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl;
std::cout << std::endl;
std::cout << "Number of correspondences: " << N << std::endl;
std::cout << "Number of outliers: " << N_OUTLIERS << std::endl;
std::cout << "Time taken (s): "
<< std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() /
1000000.0
<< std::endl;
std::cin.get();
}
运行结果
