#include <iostream>
#include "cvb/point_cloud_factory.hpp"
#include "cvb/calibrator_3d.hpp"
#include "cvb/foundation/metric_aqs12.hpp"
int main()
{
try
{
std::cout << "Estimation of a rigid body transformation (rotation and translation)\n\n";
const auto rangemapFile =
Cvb::InstallPath() + CVB_LIT(
"tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x " << rangemap->Height() << " from " << rangemapFile << ".\n";
auto aqs12 = GetAqs12();
const auto calibrationFile =
Cvb::InstallPath() + CVB_LIT(
"tutorial/Metric/Images/SICalibration.json");
calibrator->SetRangeMapIgnoreValue(0.0);
std::cout << "Dense point cloud created from rangemap and calibration file with " << cloud->NumPoints() << " points.\n\n";
calibrator->SetExtrinsicMatrix(transformation);
PrintTrafo(transformation);
PrintTrafo(transformationParameters);
if (residuals)
PrintResiduals(*residuals);
auto calibratedCloud =
Cvb::PointCloudFactory::Create(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
}
catch (const std::exception & error)
{
std::cout << error.what() << std::endl;
}
return 0;
}
{
std::array< Cvb::Point3D<double>, 12> points =
{ {
{20.0018, 44.9941, 15.0000},
{24.0018, 39.9942, 14.9994},
{23.9994, 24.9972, 15.0001},
{20.0021, 20.0035, 15.0011},
{15.9994, 25.0079, 15.0016},
{16.0000, 39.9919, 15.0010},
{20.0095, 59.9985, 4.9902},
{32.0093, 44.9958, 4.9909},
{32.0052, 19.9925, 4.9920},
{20.0021, 4.9961, 4.9939},
{ 8.0024, 19.9980, 5.0009},
{ 8.0065, 45.0009, 4.9984},
} };
}
{
std::cout << "Estimated transformation:\n";
std::cout << "translation: [";
std::cout << "transformation matrix: [\n";
std::cout << transformation.
Matrix()[0][0] <<
", " << transformation.
Matrix()[0][1] <<
", " << transformation.
Matrix()[0][2] <<
",\n";
std::cout << transformation.
Matrix()[1][0] <<
", " << transformation.
Matrix()[1][1] <<
", " << transformation.
Matrix()[1][2] <<
",\n";
std::cout << transformation.
Matrix()[2][0] <<
", " << transformation.
Matrix()[2][1] <<
", " << transformation.
Matrix()[2][2] <<
"]\n\n";
}
{
std::cout <<
"Rotation angles about X,Y,Z axis in [degree]: " << transformationParameters.
RotationAngles[0] <<
", " << transformationParameters.
RotationAngles[1] <<
", " << transformationParameters.
RotationAngles[2] <<
"\n\n";
}
{
std::cout << "Residuals:\n";
for (auto residual : residuals)
{
std::cout << residual.X() << " " << residual.Y() << " " << residual.Z() << "\n";
}
std::cout << "\n";
}
Vector3D< double > Translation() const noexcept
Matrix3D Matrix() const noexcept
static std::shared_ptr< T > Load(const String &fileName)
static std::shared_ptr< AQS12DensePointCloudSegmentor > Create(const SegmentationMethod method)
static std::unique_ptr< Image > Load(const String &fileName)
static PointCloudPtr Create(const ImagePlane &rangeMap, const Calibrator3D &calibrator, PointCloudFlags flags)
RigidBodyTransformationResult CalculateRigidBodyTransformationFromAqs12Piece(const DensePointCloud &cloud, const AQS12DensePointCloudSegmentor &segmentor, const AQS12Piece &aqs12)
std::shared_ptr< Image > ImagePtr