29 lines
801 B
C
29 lines
801 B
C
|
// Copyright 2024 Bytedance Inc. All Rights Reserved.
|
||
|
|
// Author: tianlei.richard@bytedance.com (tianlei.richard)
|
||
|
|
|
||
|
|
#include "common.h"
|
||
|
|
#include <cmath>
|
||
|
|
#include <numeric>
|
||
|
|
|
||
|
|
template <typename FloatType>
|
||
|
|
bool fequal(const FloatType a, const FloatType b) {
|
||
|
|
return std::fabs(a - b) <= std::numeric_limits<FloatType>::epsilon();
|
||
|
|
}
|
||
|
|
|
||
|
|
template <int d>
|
||
|
|
std::vector<Eigen::Vector<double, d>>
|
||
|
|
apply_transform(const TransformMatrix &mtx,
|
||
|
|
const std::vector<Eigen::Vector<double, d>> &points) {
|
||
|
|
std::vector<Eigen::Vector<double, d>> res;
|
||
|
|
for (const auto &p : points) {
|
||
|
|
auto tmp_p = Eigen::Vector<double, 1 + d>();
|
||
|
|
tmp_p << p, 1.;
|
||
|
|
tmp_p = mtx * tmp_p;
|
||
|
|
if (!fequal(tmp_p.w(), 1.)) {
|
||
|
|
tmp_p /= tmp_p.w();
|
||
|
|
}
|
||
|
|
res.push_back(tmp_p.template head<d>());
|
||
|
|
}
|
||
|
|
return res;
|
||
|
|
}
|