202 lines
6.7 KiB
C++
202 lines
6.7 KiB
C++
///////////////////////////////////////////////////////////////////////////////
|
|
// File generated by HDevelop for HALCON/C++ Version 20.11.1.0
|
|
// Non-ASCII strings in this file are encoded in local-8-bit encoding (cp936).
|
|
// Ensure that the interface encoding is set to locale encoding by calling
|
|
// SetHcppInterfaceStringEncodingIsUtf8(false) at the beginning of the program.
|
|
//
|
|
// Please note that non-ASCII characters in string constants are exported
|
|
// as octal codes in order to guarantee that the strings are correctly
|
|
// created on all systems, independent on any compiler settings.
|
|
//
|
|
// Source files with different encoding should not be mixed in one project.
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
#include <halconAlgo/Subfunction/fit_plane.h>
|
|
#include <halconAlgo/Subfunction/fit_cartesian_coordinate.h>
|
|
#include <Halcon.h>
|
|
#include <halconcpp.h>
|
|
|
|
using namespace HalconCpp;
|
|
|
|
void halcon_Correction(HTuple hv_in_X_X0Y, HTuple hv_in_Y_X0Y, HTuple hv_in_Z_X0Y, HTuple hv_in_Z_X0Y_Value,
|
|
HTuple hv_in_X_X0Z, HTuple hv_in_Y_X0Z, HTuple hv_in_Z_X0Z, HTuple hv_in_Y_X0Z_Value,
|
|
HTuple hv_in_X_Y0Z, HTuple hv_in_Y_Y0Z, HTuple hv_in_Z_Y0Z, HTuple hv_in_X_Y0Z_Value,
|
|
HTuple *hv_out_Origin, HTuple *hv_out_Result_X, HTuple *hv_out_Result_Y, HTuple *hv_out_Result_Z,
|
|
HTuple *hv_out_AngleZX, HTuple *hv_out_AngleYZ, HTuple *hv_out_AngleXY, HTuple *hv_out_Flag)
|
|
{
|
|
|
|
// Local iconic variables
|
|
|
|
// Local control variables
|
|
HTuple hv_AngleX, hv_AngleY, hv_AngleZ;
|
|
HTuple hv_out_X, hv_out_Y, hv_out_Z;
|
|
HTuple hv_X0Y, hv_X0Z, hv_Y0Z;
|
|
HTuple A1, B1, C1, D1;
|
|
HTuple A2, B2, C2, D2;
|
|
HTuple A3, B3, C3, D3;
|
|
HTuple hv_Sensor_Dst, hv_Row_Dst, hv_Col_Dst;
|
|
HTuple hv_D1, hv_D2, hv_D3, hv_MatrixID0, hv_MatrixID1;
|
|
HTuple hv_MatrixID2, hv_MatrixID3, hv_D, hv_out_Origin_X;
|
|
HTuple hv_out_Origin_Y, hv_out_Origin_Z, hv_Exception;
|
|
|
|
(*hv_out_Flag) = 1;
|
|
//185 60
|
|
try
|
|
{
|
|
//激光方向下点激光到机器人坐标位置的距离,测量范围中心，净距离
|
|
hv_Sensor_Dst = 0;
|
|
|
|
hv_Row_Dst = 0;
|
|
hv_Col_Dst = 0;
|
|
|
|
//按顺序
|
|
//X0Y坐标平面
|
|
hv_in_X_X0Y = (hv_in_X_X0Y.TupleReal())+hv_Row_Dst;
|
|
hv_in_Y_X0Y = (hv_in_Y_X0Y.TupleReal())-hv_Col_Dst;
|
|
hv_in_Z_X0Y = ((hv_in_Z_X0Y-hv_Sensor_Dst)+hv_in_Z_X0Y_Value).TupleReal();
|
|
|
|
//X0Z坐标平面
|
|
hv_in_X_X0Z = (hv_in_X_X0Z.TupleReal())-hv_Row_Dst;
|
|
hv_in_Y_X0Z = ((hv_in_Y_X0Z-hv_Sensor_Dst)+hv_in_Y_X0Z_Value).TupleReal();
|
|
hv_in_Z_X0Z = (hv_in_Z_X0Z.TupleReal())-hv_Col_Dst;
|
|
|
|
//Y0Z坐标平面
|
|
hv_in_X_Y0Z = ((hv_in_X_Y0Z-hv_Sensor_Dst)+hv_in_X_Y0Z_Value).TupleReal();
|
|
hv_in_Y_Y0Z = (hv_in_Y_Y0Z.TupleReal())-hv_Col_Dst;
|
|
hv_in_Z_Y0Z = (hv_in_Z_Y0Z.TupleReal())-hv_Row_Dst;
|
|
|
|
|
|
//拟合平面
|
|
//X0Y坐标平面
|
|
fit_plane(hv_in_X_X0Y, hv_in_Y_X0Y, hv_in_Z_X0Y, &A1, &B1, &C1, &D1);
|
|
if (C1*D1 > 0)
|
|
{
|
|
(*hv_out_Flag) = 99999;
|
|
return;
|
|
}
|
|
hv_X0Y.Append(A1);
|
|
hv_X0Y.Append(B1);
|
|
hv_X0Y.Append(C1);
|
|
|
|
fit_plane(hv_in_X_X0Z, hv_in_Y_X0Z, hv_in_Z_X0Z, &A2, &B2, &C2, &D2);
|
|
if (B2 * D2 > 0)
|
|
{
|
|
(*hv_out_Flag) = 99999;
|
|
return;
|
|
}
|
|
hv_X0Z.Append(A2);
|
|
hv_X0Z.Append(B2);
|
|
hv_X0Z.Append(C2);
|
|
|
|
fit_plane(hv_in_X_Y0Z, hv_in_Y_Y0Z, hv_in_Z_Y0Z, &A3, &B3, &C3, &D3);
|
|
if (A3 * D3 > 0)
|
|
{
|
|
(*hv_out_Flag) = 99999;
|
|
return;
|
|
}
|
|
hv_Y0Z.Append(A3);
|
|
hv_Y0Z.Append(B3);
|
|
hv_Y0Z.Append(C3);
|
|
|
|
HTuple tepMatrix1, tepMatrix2, tepMatrix3, tepMatrix4;
|
|
|
|
tepMatrix1.Append(A1);
|
|
tepMatrix1.Append(B1);
|
|
tepMatrix1.Append(C1);
|
|
|
|
tepMatrix1.Append(A2);
|
|
tepMatrix1.Append(B2);
|
|
tepMatrix1.Append(C2);
|
|
|
|
tepMatrix1.Append(A3);
|
|
tepMatrix1.Append(B3);
|
|
tepMatrix1.Append(C3);
|
|
CreateMatrix(3, 3, tepMatrix1, &hv_MatrixID0);
|
|
|
|
tepMatrix2.Append(D1);
|
|
tepMatrix2.Append(B1);
|
|
tepMatrix2.Append(C1);
|
|
|
|
tepMatrix2.Append(D2);
|
|
tepMatrix2.Append(B2);
|
|
tepMatrix2.Append(C2);
|
|
|
|
tepMatrix2.Append(D3);
|
|
tepMatrix2.Append(B3);
|
|
tepMatrix2.Append(C3);
|
|
CreateMatrix(3, 3, tepMatrix2, &hv_MatrixID1);
|
|
|
|
tepMatrix3.Append(A1);
|
|
tepMatrix3.Append(D1);
|
|
tepMatrix3.Append(C1);
|
|
|
|
tepMatrix3.Append(A2);
|
|
tepMatrix3.Append(D2);
|
|
tepMatrix3.Append(C2);
|
|
|
|
tepMatrix3.Append(A3);
|
|
tepMatrix3.Append(D3);
|
|
tepMatrix3.Append(C3);
|
|
CreateMatrix(3, 3, tepMatrix3, &hv_MatrixID2);
|
|
|
|
tepMatrix4.Append(A1);
|
|
tepMatrix4.Append(B1);
|
|
tepMatrix4.Append(D1);
|
|
|
|
tepMatrix4.Append(A2);
|
|
tepMatrix4.Append(B2);
|
|
tepMatrix4.Append(D2);
|
|
|
|
tepMatrix4.Append(A3);
|
|
tepMatrix4.Append(B3);
|
|
tepMatrix4.Append(D3);
|
|
CreateMatrix(3, 3, tepMatrix4, &hv_MatrixID3);
|
|
|
|
DeterminantMatrix(hv_MatrixID0, "general", &hv_D);
|
|
DeterminantMatrix(hv_MatrixID1, "general", &hv_D1);
|
|
DeterminantMatrix(hv_MatrixID2, "general", &hv_D2);
|
|
DeterminantMatrix(hv_MatrixID3, "general", &hv_D3);
|
|
|
|
//交点坐标(产品坐标系原点)
|
|
hv_out_Origin_X = (((hv_D1.TupleReal())/hv_D).TupleString(".1f")).TupleNumber();
|
|
hv_out_Origin_Y = (((hv_D2.TupleReal())/hv_D).TupleString(".1f")).TupleNumber();
|
|
hv_out_Origin_Z = (((hv_D3.TupleReal())/hv_D).TupleString(".1f")).TupleNumber();
|
|
(*hv_out_Origin).Clear();
|
|
(*hv_out_Origin).Append(hv_out_Origin_X);
|
|
(*hv_out_Origin).Append(hv_out_Origin_Y);
|
|
(*hv_out_Origin).Append(hv_out_Origin_Z);
|
|
|
|
fit_cartesian_coordinate(hv_X0Y, hv_X0Z, hv_Y0Z, &hv_out_X, &hv_out_Y, &hv_out_Z, hv_out_AngleXY, hv_out_AngleYZ, hv_out_AngleZX, &hv_AngleX, &hv_AngleY, &hv_AngleZ);
|
|
|
|
//X轴上的点
|
|
// out_Result_X := [number((out_Origin_X+100*out_X[0])$'.1f'),number((out_Origin_Y+100*out_X[1])$'.1f'),number((out_Origin_Z+100*out_X[2])$'.1f')]
|
|
//Y轴上的点
|
|
// out_Result_Y := [number((out_Origin_X+100*out_Y[0])$'.1f'),number((out_Origin_Y+100*out_Y[1])$'.1f'),number((out_Origin_Z+100*out_Y[2])$'.1f')]
|
|
//Z轴上的点
|
|
// out_Result_Z := [number((out_Origin_X+100*out_Z[0])$'.1f'),number((out_Origin_Y+100*out_Z[1])$'.1f'),number((out_Origin_Z+100*out_Z[2])$'.1f')]
|
|
|
|
// X轴上的点
|
|
(*hv_out_Result_X).Append(hv_out_Origin_X + 100 * hv_out_X[0]);
|
|
(*hv_out_Result_X).Append(hv_out_Origin_Y + 100 * hv_out_X[1]);
|
|
(*hv_out_Result_X).Append(hv_out_Origin_Z + 100 * hv_out_X[2]);
|
|
//// Y轴上的点
|
|
(*hv_out_Result_Y).Append(hv_out_Origin_X + 100 * hv_out_Y[0]);
|
|
(*hv_out_Result_Y).Append(hv_out_Origin_Y + 100 * hv_out_Y[1]);
|
|
(*hv_out_Result_Y).Append(hv_out_Origin_Z + 100 * hv_out_Y[2]);
|
|
//// Z轴上的点
|
|
(*hv_out_Result_Z).Append(hv_out_Origin_X + 100 * hv_out_Z[0]);
|
|
(*hv_out_Result_Z).Append(hv_out_Origin_Y + 100 * hv_out_Z[1]);
|
|
(*hv_out_Result_Z).Append(hv_out_Origin_Z + 100 * hv_out_Z[2]);
|
|
|
|
}
|
|
// catch (Exception)
|
|
catch (HException &HDevExpDefaultException)
|
|
{
|
|
HDevExpDefaultException.ToHTuple(&hv_Exception);
|
|
(*hv_out_Flag) = 99999;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|