source: rrlib_model_fitting/tests/icp.cpp @ 70:49ba3e566e7b

17.03 tip
Last change on this file since 70:49ba3e566e7b was 70:49ba3e566e7b, checked in by Tobias Föhst <foehst@…>, 23 months ago

Adds missing include

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#include <iomanip>
40
41//----------------------------------------------------------------------
42// Internal includes with ""
43//----------------------------------------------------------------------
44
45//----------------------------------------------------------------------
46// Debugging
47//----------------------------------------------------------------------
48#include <cassert>
49
50//----------------------------------------------------------------------
51// Namespace usage
52//----------------------------------------------------------------------
53
54//----------------------------------------------------------------------
55// Namespace declaration
56//----------------------------------------------------------------------
57namespace rrlib
58{
59namespace model_fitting
60{
61
62//----------------------------------------------------------------------
63// Forward declarations / typedefs / enums
64//----------------------------------------------------------------------
65
66//----------------------------------------------------------------------
67// Const values
68//----------------------------------------------------------------------
69
70//----------------------------------------------------------------------
71// Implementation
72//----------------------------------------------------------------------
73class TestICP : public util::tUnitTestSuite
74{
75  RRLIB_UNIT_TESTS_BEGIN_SUITE(TestICP);
76  RRLIB_UNIT_TESTS_ADD_TEST(RelativeMotion);
77  RRLIB_UNIT_TESTS_ADD_TEST(EmptyPointSets);
78  RRLIB_UNIT_TESTS_END_SUITE;
79
80private:
81
82  void RelativeMotion()
83  {
84    std::vector<math::tVec2d> world_data;
85    world_data.emplace_back(200, 150);
86    world_data.emplace_back(300, 300);
87    world_data.emplace_back(500, 100);
88
89    math::tPose2D sensor_pose(10, 10, math::tAngleDeg(20));
90
91    std::vector<math::tVec2d> data;
92    for (auto & d : world_data)
93    {
94      data.emplace_back(math::tPose2D(d).GetPoseInLocalFrame(sensor_pose).Position());
95    }
96
97    math::tPose2D motion(5, 0, math::tAngleDeg(10));
98    auto new_sensor_pose = motion.GetPoseInParentFrame(sensor_pose);
99
100    std::vector<math::tVec2d> model;
101    for (auto & d : world_data)
102    {
103      model.emplace_back(math::tPose2D(d).GetPoseInLocalFrame(new_sensor_pose).Position());
104    }
105
106    tIterativeClosestPoint2D icp;
107    icp.SetModel(model.begin(), model.end());
108    icp.SetData(data.begin(), data.end());
109    RRLIB_UNIT_TESTS_ASSERT(icp.DoICP(1E-13));
110
111    math::tPose2D result(icp.Transformation());
112    std::ostringstream output;
113    output << std::setprecision(10) << result << " != " << motion;
114
115    RRLIB_UNIT_TESTS_ASSERT_MESSAGE(output.str(), IsEqual(result, motion, 1E-6));
116  }
117
118  void EmptyPointSets()
119  {
120    std::vector<math::tVec2d> data {math::tVec2d(0, 0)};
121    std::vector<math::tVec2d> model {math::tVec2d(1, 1)};;
122
123    tIterativeClosestPoint2D icp;
124    icp.SetModel(model.begin(), model.begin());
125    icp.SetData(data.begin(), data.begin());
126    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
127
128    icp.SetModel(model.begin(), model.end());
129    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
130
131    icp.SetModel(model.begin(), model.begin());
132    icp.SetData(data.begin(), data.end());
133    RRLIB_UNIT_TESTS_ASSERT(!icp.DoICP());
134
135    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.begin(), data.begin(), data.begin()), std::runtime_error);
136    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.end(), data.begin(), data.begin()), std::runtime_error);
137    RRLIB_UNIT_TESTS_EXCEPTION(tIterativeClosestPoint2D(model.begin(), model.begin(), data.begin(), data.end()), std::runtime_error);
138  }
139
140};
141
142RRLIB_UNIT_TESTS_REGISTER_SUITE(TestICP);
143
144//----------------------------------------------------------------------
145// End of namespace declaration
146//----------------------------------------------------------------------
147}
148}
Note: See TracBrowser for help on using the repository browser.