CVB++ 14.1
Foundation/CppMetricCalibration
1// ---------------------------------------------------------------------------
17// ---------------------------------------------------------------------------
18
19
20#include <iostream>
21
22#include "cvb/point_cloud_factory.hpp"
23#include "cvb/calibrator_3d.hpp"
24#include "cvb/foundation/metric_aqs12.hpp"
25
26
28void PrintTrafo(const Cvb::AffineMatrix3D& trafo);
29void PrintResiduals(const std::array<Cvb::Point3D<double>, 12>& residuals);
30void PrintAqs12Points(const std::vector< Cvb::Point3D<double>>& points);
31bool CheckAccuracy(const std::array<Cvb::Point3D<double>, 12>& residuals, double desiredAccuracy);
32
33int main()
34{
35 try
36 {
37 std::cout << "Estimation of homography and affine transformation (correcting an inclined laser plane)\n\n";
38
39 // load range map of AQS12
40 const auto rangemapFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
41 Cvb::ImagePtr rangemap = Cvb::Image::Load(rangemapFile);
42 std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x " << rangemap->Height() << " from " << rangemapFile << ".\n\n";
43
44 // create calibration configuration object
45 auto aqs12 = GetAqs12();
47
48 // create AQS12 segmentor for range maps
50
51 // estimate calibration parameters
54 std::tie(calibrator, residuals) = Cvb::Foundation::Metric::CreateCalibratorFromAqs12Piece(rangemap->Plane(0), *segmentor, *config);
55
56 // show results
57 if(auto trafo = calibrator->CorrectionOfLaserPlaneInclination())
58 PrintTrafo(*trafo);
59 PrintResiduals(residuals);
60
61 // check intermediate results if residuals are not ok
62 double desiredAccuracy = 0.05;
63 if (!CheckAccuracy(residuals, desiredAccuracy))
64 {
65 std::cout << "Results do not have desired accuracy. Check face segmentation and extracted AQS12 points...\n";
66 auto facesAqs12 = segmentor->FaceSegmentationFromPiece(rangemap->Plane(0));
67 auto pointsAqs12 = segmentor->ExtractProjectedPointsFromPiece(rangemap->Plane(0));
68 PrintAqs12Points(pointsAqs12);
69 }
70 else
71 {
72 // create calibrated cloud
73 auto calibratedCloud = Cvb::PointCloudFactory::Create(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
74 std::cout << "The calibration was sucessful and accuracy is < " << desiredAccuracy << " mm. :)\n";
75 }
76 }
77 catch (const std::exception & error)
78 {
79 std::cout << error.what() << std::endl;
80 }
81
82 return 0;
83}
84
86{
87 // reference points in mm in a right-handed coordindate system
89 { {
90 {20.0018, 44.9941, 15.0000},
91 {24.0018, 39.9942, 14.9994},
92 {23.9994, 24.9972, 15.0001},
93 {20.0021, 20.0035, 15.0011},
94 {15.9994, 25.0079, 15.0016},
95 {16.0000, 39.9919, 15.0010},
96 {20.0095, 59.9985, 4.9902},
97 {32.0093, 44.9958, 4.9909},
98 {32.0052, 19.9925, 4.9920},
99 {20.0021, 4.9961, 4.9939},
100 { 8.0024, 19.9980, 5.0009},
101 { 8.0065, 45.0009, 4.9984},
102 } };
103
105}
106
107bool CheckAccuracy(const std::array<Cvb::Point3D<double>, 12>& residuals, double desiredAccuracy)
108{
109 for (auto residual : residuals)
110 {
111 if (std::abs(residual.X()) > desiredAccuracy || std::abs(residual.Y()) > desiredAccuracy || std::abs(residual.Z()) > desiredAccuracy)
112 {
113 return false;
114 }
115 }
116 return true;
117}
118
119void PrintTrafo(const Cvb::AffineMatrix3D& trafo)
120{
121 std::cout << "Estimated transformation:\n";
122 std::cout << "translation: [";
123 std::cout << trafo.Translation().X() << ", " << trafo.Translation().Y() << ", " << trafo.Translation().Z() << "]\n";
124
125 std::cout << "transformation matrix: [\n";
126 std::cout << trafo.Matrix()[0][0] << ", " << trafo.Matrix()[0][1] << ", " << trafo.Matrix()[0][2] << ",\n";
127 std::cout << trafo.Matrix()[1][0] << ", " << trafo.Matrix()[1][1] << ", " << trafo.Matrix()[1][2] << ",\n";
128 std::cout << trafo.Matrix()[2][0] << ", " << trafo.Matrix()[2][1] << ", " << trafo.Matrix()[2][2] << "]\n\n";
129}
130
131void PrintResiduals(const std::array<Cvb::Point3D<double>, 12>& residuals)
132{
133 std::cout << "Residuals:\n";
134 for (auto residual : residuals)
135 {
136 std::cout << residual.X() << " " << residual.Y() << " " << residual.Z() << "\n";
137 }
138 std::cout << "\n";
139}
140
141void PrintAqs12Points(const std::vector< Cvb::Point3D<double>>& points)
142{
143 std::cout << "AQS12 points\n";
144 for (auto point : points)
145 {
146 std::cout << point.X() << " " << point.Y() << " " << point.Z() << "\n";
147 }
148 std::cout << "\n";
149}
Affine transformation for 3D containing a transformation matrix and a translation vector.
Definition: affine_matrix_3d.hpp:97
Vector3D< double > Translation() const noexcept
Gets the translation part of the transformation.
Definition: affine_matrix_3d.hpp:156
Matrix3D Matrix() const noexcept
Gets the matrix part of the transformation.
Definition: affine_matrix_3d.hpp:136
Object to collect all input parameters for the AQS12 calibration piece.
Definition: decl_metric_aqs12_calibration_piece.hpp:28
static std::shared_ptr< AQS12RangeMapSegmentor > Create(const SegmentationMethod method)
Creates a AQS12 segmentor for range maps based on given segmentation method.
Definition: decl_metric_segmentor_range_map_aqs12.hpp:35
static std::unique_ptr< CalibrationConfiguration > Create(const AQS12Piece &aqs12)
Creates a calibration configuration object.
Definition: decl_metric_calibration_configuration.hpp:56
static std::unique_ptr< Image > Load(const String &fileName)
Loads an image with the given file name.
Definition: detail_image.hpp:32
static PointCloudPtr Create(const ImagePlane &rangeMap, const Calibrator3D &calibrator, PointCloudFlags flags)
Creates a new Cartesian 3D point cloud from the given 2.5D range map image.
Definition: point_cloud_factory.hpp:87
@ KmeansClustering
Clustering top, base and faces using kmeans.
AQS12CalibratorResult CreateCalibratorFromAqs12Piece(const ImagePlane &imagePlane, const AQS12RangeMapSegmentor &segmentor, const CalibrationConfiguration &config)
Calculates intrinsic calibration parameters from the given range map image of an AQS12 calibration pi...
Definition: metric_aqs12.hpp:68