1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
|
/************************************************************************
*
* Copyright (C) 2018-2023 IRCAD France
* Copyright (C) 2018 IHU Strasbourg
*
* This file is part of Sight.
*
* Sight is free software: you can redistribute it and/or modify it under
* the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Sight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with Sight. If not, see <https://www.gnu.org/licenses/>.
*
***********************************************************************/
#include "tool_calibration.hpp"
#include <geometry/vision/helper.hpp>
#include <service/macros.hpp>
namespace sight::module::navigation::calibration
{
static const service::base::key_t MATRIX_CALIBRATION_OUTPUT = "matrixCalibration";
// -----------------------------------------------------------------------------
tool_calibration::tool_calibration() noexcept =
default;
// -----------------------------------------------------------------------------
tool_calibration::~tool_calibration() noexcept =
default;
// -----------------------------------------------------------------------------
void tool_calibration::configuring()
{
const auto config_tree = this->get_config();
const auto outputs = config_tree.equal_range("out");
for(auto it = outputs.first ; it != outputs.second ; ++it)
{
const auto key = it->second.get<std::string>("<xmlattr>.key");
if(key == MATRIX_CENTER_OUTPUT)
{
m_has_output_center = true;
}
}
}
// -----------------------------------------------------------------------------
void tool_calibration::starting()
{
}
// -----------------------------------------------------------------------------
void tool_calibration::stopping()
{
m_matrix_calibration = nullptr;
if(m_has_output_center)
{
m_matrix_center = nullptr;
}
}
// -----------------------------------------------------------------------------
void tool_calibration::updating()
{
}
// -----------------------------------------------------------------------------
void tool_calibration::compute_registration(core::clock::type /*timestamp*/)
{
const auto matrices_vector = m_matrices_vector.lock();
data::matrix4::sptr calibration_matrix = std::make_shared<data::matrix4>();
data::matrix4::sptr center_matrix_no_rot = std::make_shared<data::matrix4>();
geometry::vision::helper::calibrate_pointing_tool(
matrices_vector.get_shared(),
calibration_matrix,
center_matrix_no_rot
);
m_matrix_calibration = calibration_matrix;
if(m_has_output_center)
{
m_matrix_center = center_matrix_no_rot;
}
}
} // namespace sight::module::navigation::calibration
|
