CVB++ 15.0
Foundation/CppMetricCalibrationRigidBodyTrafo
1// ---------------------------------------------------------------------------
12// ---------------------------------------------------------------------------
13
14
15#include <iostream>
16
17#include "cvb/point_cloud_factory.hpp"
18#include "cvb/calibrator_3d.hpp"
19#include "cvb/foundation/metric_aqs12.hpp"
20
22void PrintTrafo(const Cvb::AffineMatrix3D& transformation);
23void PrintTrafo(const Cvb::AffineTransformationParameters& transformationParameters);
24void PrintResiduals(const std::vector<Cvb::Point3D<double>>& residuals);
25
26int main()
27{
28 try
29 {
30 std::cout << "Estimation of a rigid body transformation (rotation and translation)\n\n";
31
32 // load range map of AQS12
33 const auto rangemapFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
34 Cvb::ImagePtr rangemap = Cvb::Image::Load(rangemapFile);
35 std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x " << rangemap->Height() << " from " << rangemapFile << ".\n";
36
37 // create calibration configuration object
38 auto aqs12 = GetAqs12();
39
40 // create intrinsically calibrated dense point cloud
41 const auto calibrationFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/SICalibration.json"); // calibration file with homography and scaling
42 auto calibrator = Cvb::Calibrator3D::Load<Cvb::Calibrator3D>(calibrationFile);
43 calibrator->SetRangeMapIgnoreValue(0.0);
44 auto cloud = Cvb::PointCloudFactory::Create<Cvb::DensePointCloud>(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
45 std::cout << "Dense point cloud created from rangemap and calibration file with " << cloud->NumPoints() << " points.\n\n";
46
47 // create AQS12 segmentor for dense point clouds
49
50 // estimate calibration parameters
51 auto [transformation, residuals, transformationParameters] = Cvb::Foundation::Metric::CalculateRigidBodyTransformationFromAqs12Piece(*cloud, *segmentor, aqs12);
52 calibrator->SetExtrinsicMatrix(transformation);
53
54 // show results
55 PrintTrafo(transformation);
56 PrintTrafo(transformationParameters);
57 if (residuals)
58 PrintResiduals(*residuals);
59
60 // create calibrated cloud
61 auto calibratedCloud = Cvb::PointCloudFactory::Create(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
62 }
63 catch (const std::exception & error)
64 {
65 std::cout << error.what() << std::endl;
66 }
67
68 return 0;
69}
70
72{
73 // reference points in mm in a right-handed coordinate system
75 { {
76 {20.0018, 44.9941, 15.0000},
77 {24.0018, 39.9942, 14.9994},
78 {23.9994, 24.9972, 15.0001},
79 {20.0021, 20.0035, 15.0011},
80 {15.9994, 25.0079, 15.0016},
81 {16.0000, 39.9919, 15.0010},
82 {20.0095, 59.9985, 4.9902},
83 {32.0093, 44.9958, 4.9909},
84 {32.0052, 19.9925, 4.9920},
85 {20.0021, 4.9961, 4.9939},
86 { 8.0024, 19.9980, 5.0009},
87 { 8.0065, 45.0009, 4.9984},
88 } };
89
91}
92
93void PrintTrafo(const Cvb::AffineMatrix3D& transformation)
94{
95 std::cout << "Estimated transformation:\n";
96 std::cout << "translation: [";
97 std::cout << transformation.Translation().X() << ", " << transformation.Translation().Y() << ", " << transformation.Translation().Z() << "]\n";
98
99 std::cout << "transformation matrix: [\n";
100 std::cout << transformation.Matrix()[0][0] << ", " << transformation.Matrix()[0][1] << ", " << transformation.Matrix()[0][2] << ",\n";
101 std::cout << transformation.Matrix()[1][0] << ", " << transformation.Matrix()[1][1] << ", " << transformation.Matrix()[1][2] << ",\n";
102 std::cout << transformation.Matrix()[2][0] << ", " << transformation.Matrix()[2][1] << ", " << transformation.Matrix()[2][2] << "]\n\n";
103}
104
105void PrintTrafo(const Cvb::AffineTransformationParameters& transformationParameters)
106{
107 std::cout << "Rotation angles about X,Y,Z axis in [degree]: " << transformationParameters.RotationAngles[0] << ", " << transformationParameters.RotationAngles[1] << ", " << transformationParameters.RotationAngles[2] << "\n\n";
108}
109
110void PrintResiduals(const std::vector<Cvb::Point3D<double>>& residuals)
111{
112 std::cout << "Residuals:\n";
113 for (auto residual : residuals)
114 {
115 std::cout << residual.X() << " " << residual.Y() << " " << residual.Z() << "\n";
116 }
117 std::cout << "\n";
118}
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< 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.
RigidBodyTransformationResult CalculateRigidBodyTransformationFromAqs12Piece(const DensePointCloud &cloud, const AQS12DensePointCloudSegmentor &segmentor, const AQS12Piece &aqs12)
Calculates a rigid body transformation from a given dense point cloud of an AQS12 calibration piece.
Definition: metric_aqs12.hpp:235
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