CVB++ 14.1
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& trafo);
23
24int main()
25{
26 try
27 {
28 std::cout << "Estimation of a rigid body transformation (rotation and translation)\n\n";
29
30 // load range map of AQS12
31 const auto rangemapFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
32 Cvb::ImagePtr rangemap = Cvb::Image::Load(rangemapFile);
33 std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x " << rangemap->Height() << " from " << rangemapFile << ".\n";
34
35 // create calibration configuration object
36 auto aqs12 = GetAqs12();
37
38 // create intrinsically calibrated dense point cloud
39 const auto calibrationFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/SICalibration.json"); // calibration file with homography and scaling
40 auto calibrator = Cvb::Calibrator3D::Load<Cvb::Calibrator3D>(calibrationFile);
41 calibrator->SetRangeMapIgnoreValue(0.0);
42 auto cloud = Cvb::PointCloudFactory::Create<Cvb::DensePointCloud>(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
43 std::cout << "Dense point cloud created from rangemap and calibration file with " << cloud->NumPoints() << " points.\n\n";
44
45 // create AQS12 segmentor for dense point clouds
47
48 // estimate calibration parameters
51 std::tie(trafo,residuals) = Cvb::Foundation::Metric::CalculateRigidBodyTransformationFromAqs12Piece(*cloud, *segmentor, aqs12);
52 calibrator->SetExtrinsicMatrix(trafo);
53
54 // show results
55 PrintTrafo(trafo);
56
57 // create calibrated cloud
58 auto calibratedCloud = Cvb::PointCloudFactory::Create(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
59 }
60 catch (const std::exception & error)
61 {
62 std::cout << error.what() << std::endl;
63 }
64
65 return 0;
66}
67
69{
70 // reference points in mm in a right-handed coordindate system
72 { {
73 {20.0018, 44.9941, 15.0000},
74 {24.0018, 39.9942, 14.9994},
75 {23.9994, 24.9972, 15.0001},
76 {20.0021, 20.0035, 15.0011},
77 {15.9994, 25.0079, 15.0016},
78 {16.0000, 39.9919, 15.0010},
79 {20.0095, 59.9985, 4.9902},
80 {32.0093, 44.9958, 4.9909},
81 {32.0052, 19.9925, 4.9920},
82 {20.0021, 4.9961, 4.9939},
83 { 8.0024, 19.9980, 5.0009},
84 { 8.0065, 45.0009, 4.9984},
85 } };
86
88}
89
90void PrintTrafo(const Cvb::AffineMatrix3D& trafo)
91{
92 std::cout << "Estimated transformation:\n";
93 std::cout << "translation: [";
94 std::cout << trafo.Translation().X() << ", " << trafo.Translation().Y() << ", " << trafo.Translation().Z() << "]\n";
95
96 std::cout << "transformation matrix: [\n";
97 std::cout << trafo.Matrix()[0][0] << ", " << trafo.Matrix()[0][1] << ", " << trafo.Matrix()[0][2] << ",\n";
98 std::cout << trafo.Matrix()[1][0] << ", " << trafo.Matrix()[1][1] << ", " << trafo.Matrix()[1][2] << ",\n";
99 std::cout << trafo.Matrix()[2][0] << ", " << trafo.Matrix()[2][1] << ", " << trafo.Matrix()[2][2] << "]\n\n";
100}
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< 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.
AQS12TransformationResult CalculateRigidBodyTransformationFromAqs12Piece(const DensePointCloud &cloud, const AQS12DensePointCloudSegmentor &segmentor, const AQS12Piece &aqs12)
Calculates the rigid body transformation from the given dense point cloud of an AQS12 calibration pie...
Definition: metric_aqs12.hpp:197