source: rrlib_model_fitting/tests/icp.cpp @ 65:e701c562d80f

17.03
Last change on this file since 65:e701c562d80f was 65:e701c562d80f, checked in by Tobias Föhst <tobias.foehst@…>, 3 years ago

Adds unit test for ICP

File size: 5.1 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    rrlib/model_fitting/tests/icp.cpp
23 *
24 * \author  Tobias Föhst
25 *
26 * \date    2017-04-25
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include "rrlib/util/tUnitTestSuite.h"
35
36#include "rrlib/model_fitting/tIterativeClosestPoint.h"
37#include "rrlib/math/tPose2D.h"
38
39//----------------------------------------------------------------------
40// Internal includes with ""
41//----------------------------------------------------------------------
42
43//----------------------------------------------------------------------
44// Debugging
45//----------------------------------------------------------------------
46#include <cassert>
47
48//----------------------------------------------------------------------
49// Namespace usage
50//----------------------------------------------------------------------
51
52//----------------------------------------------------------------------
53// Namespace declaration
54//----------------------------------------------------------------------
55namespace rrlib
56{
57namespace model_fitting
58{
59
60//----------------------------------------------------------------------
61// Forward declarations / typedefs / enums
62//----------------------------------------------------------------------
63
64//----------------------------------------------------------------------
65// Const values
66//----------------------------------------------------------------------
67
68//----------------------------------------------------------------------
69// Implementation
70//----------------------------------------------------------------------
71class TestICP : public util::tUnitTestSuite
72{
73  RRLIB_UNIT_TESTS_BEGIN_SUITE(TestICP);
74  RRLIB_UNIT_TESTS_ADD_TEST(RelativeMotion);
75  RRLIB_UNIT_TESTS_ADD_TEST(EmptyPointSets);
76  RRLIB_UNIT_TESTS_END_SUITE;
77
78private:
79
80  void RelativeMotion()
81  {
82    std::vector<math::tVec2d> world_data;
83    world_data.emplace_back(200, 150);
84    world_data.emplace_back(300, 300);
85    world_data.emplace_back(500, 100);
86
87    math::tPose2D sensor_pose(10, 10, math::tAngleDeg(20));
88
89    std::vector<math::tVec2d> data;
90    for (auto & d : world_data)
91    {
92      data.emplace_back(math::tPose2D(d).GetPoseInLocalFrame(sensor_pose).Position());
93    }
94
95    math::tPose2D motion(5, 0, math::tAngleDeg(10));
96    auto new_sensor_pose = motion.GetPoseInParentFrame(sensor_pose);
97
98    std::vector<math::tVec2d> model;
99    for (auto & d : world_data)
100    {
101      model.emplace_back(math::tPose2D(d).GetPoseInLocalFrame(new_sensor_pose).Position());
102    }
103
104    tIterativeClosestPoint2D icp;
105    icp.SetModel(model.begin(), model.end());
106    icp.SetData(data.begin(), data.end());
107    RRLIB_UNIT_TESTS_ASSERT(icp.DoICP(1E-13));
108
109    math::tPose2D result(icp.Transformation());
110    std::ostringstream output;
111    output << std::setprecision(10) << result << " != " << motion;
112
113    RRLIB_UNIT_TESTS_ASSERT_MESSAGE(output.str(), IsEqual(result, motion, 1E-6));
114  }
115
116  void EmptyPointSets()
117  {
118    std::vector<math::tVec2d> data {math::tVec2d(0, 0)};
119    std::vector<math::tVec2d> model {math::tVec2d(1, 1)};;
120
121    tIterativeClosestPoint2D icp;
122    icp.SetModel(model.begin(), model.begin());
123    icp.SetData(data.begin(), data.begin());
124    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
125
126    icp.SetModel(model.begin(), model.end());
127    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
128
129    icp.SetModel(model.begin(), model.begin());
130    icp.SetData(data.begin(), data.end());
131    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
132
133    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.begin(), data.begin(), data.begin()), std::runtime_error);
134    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.end(), data.begin(), data.begin()), std::runtime_error);
135    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.begin(), data.begin(), data.end()), std::runtime_error);
136  }
137
138};
139
140RRLIB_UNIT_TESTS_REGISTER_SUITE(TestICP);
141
142//----------------------------------------------------------------------
143// End of namespace declaration
144//----------------------------------------------------------------------
145}
146}
Note: See TracBrowser for help on using the repository browser.