36 #include "../../utest.h"
64 double median_coeff(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>& A){
65 Eigen::Map<Eigen::Matrix<NumericType, 1, Eigen::Dynamic>> v(A.data(),A.size());
66 std::sort(v.data(), v.data() + v.size());
75 namespace fs = boost::filesystem;
76 fs::path config_dir(
dataPath +
"icp_data");
77 EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) );
79 fs::directory_iterator end_iter;
80 for( fs::directory_iterator d(config_dir); d != end_iter; ++d)
82 if (!fs::is_regular_file(d->status()) )
continue;
84 std::cout <<
"Testing file " << d->path().string() << std::endl;
88 if (fs::extension(
config_file) !=
".yaml")
continue;
97 fs::path cur_file = d->path();
98 cur_file.replace_extension(
".cur_trans");
100 std::ofstream otfs(cur_file.c_str());
106 fs::path ref_file = d->path();
107 ref_file.replace_extension(
".ref_trans");
110 std::ifstream itfs(ref_file.c_str());
111 EXPECT_TRUE(itfs.good()) <<
"Could not find " << ref_file
112 <<
". If this is the first time this test is run, "
113 <<
"create it as a copy of " << cur_file;
115 for (
int row = 0; row < refT.cols(); row++)
117 for (
int col = 0; col < refT.cols(); col++)
119 itfs >>refT(row, col);
136 Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> AbsDiff = (curT*
data.features - refT*
data.features).array().abs();
140 Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> Data = (curT*
data.features).array().abs();
144 double rel_err = median_diff/median_data;
148 EXPECT_LT(rel_err, 0.05) <<
"This error was caused by the test file:" << endl <<
" " <<
config_file;
158 const size_t nX = 10, nY = nX;
159 Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> points(4, nX * nY);
161 const NumericType oX = -(nX * d / 2), oY = -(nY * d / 2);
163 for(
size_t x = 0;
x < nX;
x++){
164 for(
size_t y = 0; y < nY; y++){
165 points.col(
x * nY + y) << d *
x + oX, d * y + oY, 0, 1;
172 points.row(2).setOnes();
185 PM::Matrix expectedT = PM::Matrix::Identity(4,4);
187 EXPECT_TRUE(expectedT.isApprox(curT)) <<
"Expecting pure translation in z-direction of unit distance." << endl;
209 EXPECT_TRUE(curT.isApprox(PM::Matrix::Identity(4, 4), epsilon)) <<
"Expecting identity transform." << endl;
212 TEST(icpTest, similarityTransform)
230 double scale =
pow(curT.determinant(), 1.0/3.0);
231 EXPECT_LT( std::abs(scale - 1.04), 0.001)
232 <<
"Expecting the similarity transform scale to be 1.04.";
245 std::ifstream ifs((
dataPath +
"default.yaml").c_str());
246 icpSequence.loadFromYaml(ifs);
250 DP map = icpSequence.getPrefilteredInternalMap();
254 map = icpSequence.getPrefilteredMap();
258 icpSequence.setMap(pts0);
259 map = icpSequence.getPrefilteredInternalMap();
264 Ticp = icpSequence(pts1);
265 map = icpSequence.getPrefilteredMap();
270 Ticp = icpSequence(pts2);
271 map = icpSequence.getPrefilteredMap();
276 icpSequence.clearMap();
277 map = icpSequence.getPrefilteredInternalMap();
308 validate2dTransformation();
309 validate3dTransformation();