CVB++ 15.0
Foundation/CppMetricCalibrationInclinationLaserPlane
1// ---------------------------------------------------------------------------
17// ---------------------------------------------------------------------------
18
19
20#include <iostream>
21
22#include "cvb/point_cloud_factory.hpp"
23#include "cvb/calibrator_3d.hpp"
24
25#include "cvb/foundation/metric_aqs12.hpp"
26
27
29void PrintTrafo(const Cvb::AffineMatrix3D& transformation);
30void PrintTrafo(const Cvb::AffineTransformationParameters& transformationParameters);
31void PrintResiduals(const std::vector<Cvb::Point3D<double>>& residuals);
32void PrintAqs12Points(const std::vector< Cvb::Point3D<double>>& points);
33bool CheckAccuracy(const std::vector<Cvb::Point3D<double>>& residuals, double desiredAccuracy);
34
35
36int main()
37{
38 try
39 {
40 std::cout << "Estimation of an affine transformation (correcting an inclined laser plane)\n\n";
41
42 // load range map of AQS12
43 const auto rangemapFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
44 Cvb::ImagePtr rangemap = Cvb::Image::Load(rangemapFile);
45 std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x " << rangemap->Height() << " from " << rangemapFile << ".\n";
46
47 // create calibration configuration object
48 auto aqs12 = GetAqs12();
50 config->SetHomographyCalculated(false);
51
52 // create intrinsically calibrated dense point cloud
53 const auto calibrationFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/SICalibration.json"); // calibration file with homography and scaling
54 auto calibrator = Cvb::Calibrator3D::Load<Cvb::LaserPlaneCalibrator3D>(calibrationFile);
55 calibrator->SetRangeMapIgnoreValue(0.0);
56 auto cloud = Cvb::PointCloudFactory::Create<Cvb::DensePointCloud>(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
57 std::cout << "Dense point cloud created from rangemap and calibration file with " << cloud->NumPoints() << " points.\n\n";
58
59 // create AQS12 segmentor for dense point clouds
61
62 // estimate calibration parameters
63 auto [transformation, residuals, transformationParameters] = Cvb::Foundation::Metric::CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(*cloud, *segmentor, *config);
64 calibrator->SetCorrectionOfLaserPlaneInclination(transformation, transformationParameters);
65
66 // show results
67 PrintTrafo(transformation);
68 if (transformationParameters)
69 PrintTrafo(*transformationParameters);
70
71 // check intermediate results if residuals are not ok
72 double desiredAccuracy = 0.05;
73 if (residuals)
74 {
75 PrintResiduals(*residuals);
76 if (!CheckAccuracy(*residuals, desiredAccuracy))
77 {
78 std::cout << "Results do not have desired accuracy. Check face segmentation and extracted AQS12 points...\n";
79 auto facesAqs12 = segmentor->FaceSegmentationFromPiece(*cloud);
80 auto pointsAqs12 = segmentor->ExtractProjectedPointsFromPiece(*cloud);
81 PrintAqs12Points(pointsAqs12);
82 }
83 else
84 {
85 // create calibrated cloud
86 auto calibratedCloud = Cvb::PointCloudFactory::Create(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
87 std::cout << "The calibration was successful and accuracy is < " << desiredAccuracy << " mm. :)\n";
88 }
89 }
90
91 }
92 catch (const std::exception & error)
93 {
94 std::cout << error.what() << std::endl;
95 }
96
97 return 0;
98}
99
101{
102 // reference points in mm in a right-handed coordinate system
104 { {
105 {20.0018, 44.9941, 15.0000},
106 {24.0018, 39.9942, 14.9994},
107 {23.9994, 24.9972, 15.0001},
108 {20.0021, 20.0035, 15.0011},
109 {15.9994, 25.0079, 15.0016},
110 {16.0000, 39.9919, 15.0010},
111 {20.0095, 59.9985, 4.9902},
112 {32.0093, 44.9958, 4.9909},
113 {32.0052, 19.9925, 4.9920},
114 {20.0021, 4.9961, 4.9939},
115 { 8.0024, 19.9980, 5.0009},
116 { 8.0065, 45.0009, 4.9984},
117 } };
118
120}
121
122bool CheckAccuracy(const std::vector<Cvb::Point3D<double>>& residuals, double desiredAccuracy)
123{
124 for (auto residual : residuals)
125 {
126 if (std::abs(residual.X()) > desiredAccuracy || std::abs(residual.Y()) > desiredAccuracy || std::abs(residual.Z()) > desiredAccuracy)
127 {
128 return false;
129 }
130 }
131 return true;
132}
133
134void PrintTrafo(const Cvb::AffineMatrix3D& transformation)
135{
136 std::cout << "Estimated transformation:\n";
137 std::cout << "translation: [";
138 std::cout << transformation.Translation().X() << ", " << transformation.Translation().Y() << ", " << transformation.Translation().Z() << "]\n";
139
140 std::cout << "transformation matrix: [\n";
141 std::cout << transformation.Matrix()[0][0] << ", " << transformation.Matrix()[0][1] << ", " << transformation.Matrix()[0][2] << ",\n";
142 std::cout << transformation.Matrix()[1][0] << ", " << transformation.Matrix()[1][1] << ", " << transformation.Matrix()[1][2] << ",\n";
143 std::cout << transformation.Matrix()[2][0] << ", " << transformation.Matrix()[2][1] << ", " << transformation.Matrix()[2][2] << "]\n\n";
144}
145
146void PrintTrafo(const Cvb::AffineTransformationParameters& transformationParameters)
147{
148 std::cout << "Rotation angles about X,Y,Z axis in [degree]: " << transformationParameters.RotationAngles[0] << ", " << transformationParameters.RotationAngles[1] << ", " << transformationParameters.RotationAngles[2] << "\n";
149 std::cout << "Shear Syx, Syz: " << transformationParameters.Syx << ", " << transformationParameters.Syz << "\n";
150 std::cout << "Inclination of laser plane about X,Z axis in [degree]: " << transformationParameters.InclinationX << ", " << transformationParameters.InclinationZ << "\n";
151 std::cout << "Scale in X,Y,Z: " << transformationParameters.Scale.X << ", " << transformationParameters.Scale.Y << ", " << transformationParameters.Scale.Z << "\n\n";
152}
153
154void PrintResiduals(const std::vector<Cvb::Point3D<double>>& residuals)
155{
156 std::cout << "Residuals:\n";
157 for (auto residual : residuals)
158 {
159 std::cout << residual.X() << " " << residual.Y() << " " << residual.Z() << "\n";
160 }
161 std::cout << "\n";
162}
163
164void PrintAqs12Points(const std::vector< Cvb::Point3D<double>>& points)
165{
166 std::cout << "AQS12 points\n";
167 for (auto point: points)
168 {
169 std::cout << point.X() << " " << point.Y() << " " << point.Z() << "\n";
170 }
171 std::cout << "\n";
172}
Affine transformation for 3D containing a transformation matrix and a translation vector.
Definition: affine_matrix_3d.hpp:147
Object to collect all input parameters for the AQS12 calibration piece.
Definition: decl_metric_aqs12_calibration_piece.hpp:28
static std::shared_ptr< AQS12DensePointCloudSegmentor > Create(const SegmentationMethod method)
Creates a AQS12 segmentor for dense point clouds based on given segmentation method.
Definition: decl_metric_segmentor_dense_point_cloud_aqs12.hpp:38
static std::unique_ptr< CalibrationConfiguration > Create(const AQS12Piece &aqs12)
Creates a calibration configuration object.
Definition: decl_metric_calibration_configuration.hpp:112
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
TransformationResult CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(const DensePointCloud &cloud, const AQS12DensePointCloudSegmentor &segmentor, const CalibrationConfiguration &config)
Calculates an extrinsic calibration and the correction for the laser plane inclination (affine transf...
Definition: metric_aqs12.hpp:146
@ KmeansClustering
Clustering top, base and faces using kmeans.
Parameters of a 3D affine transformation matrix correcting an inclined laser plane.
Definition: affine_matrix_3d.hpp:37
Vector3D< double > RotationAngles
Rotation angles about x, y and z in [degree].
Definition: affine_matrix_3d.hpp:39
double InclinationZ
Inclination of laser plane (rotation about z axis) in [degree].
Definition: affine_matrix_3d.hpp:54
double Syx
Shear Syx (induced by InclinationZ).
Definition: affine_matrix_3d.hpp:45
double InclinationX
Inclination of laser plane (rotation about x axis) in [degree].
Definition: affine_matrix_3d.hpp:51
Factors3D Scale
Scale factors.
Definition: affine_matrix_3d.hpp:42
double Syz
Shear Syz (induced by InclinationX).
Definition: affine_matrix_3d.hpp:48
double Z
The Z factor.
Definition: core_3d.hpp:23
double X
The X factor.
Definition: core_3d.hpp:19
double Y
The Y factor.
Definition: core_3d.hpp:21