jerryzeng/camAlgo
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of http://snsopush.com:10100/jerryzeng/camAlgo

Browse Source
This commit is contained in:
Motorman 2025-08-24 23:46:21 +08:00
commit 83a90151b1
12 changed files with 639 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CamAlgo.sln
View File

@ -12,6 +12,9 @@ EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW", "camAlgoSW\camAlgoSW.vcxproj", "{C280CB49-2B9B-43B9-A99E-5C90B7CEF033}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW_test", "camAlgoSW_test\camAlgoSW_test.vcxproj", "{AA8276D9-77FD-451F-BC95-AC8E77D9CEFC}"
ProjectSection(ProjectDependencies) = postProject
{C280CB49-2B9B-43B9-A99E-5C90B7CEF033} = {C280CB49-2B9B-43B9-A99E-5C90B7CEF033}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camCalib", "camCalib\camCalib.vcxproj", "{3C3A4670-25E6-4E17-9723-2897316D2437}"
EndProject

2
camAlgoSW/camAlgoSW.vcxproj
View File

@ -97,10 +97,12 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<IncludePath>.\sourceCode;.\sourceCode\CG_frontEnd\inc;$(VC_IncludePath);$(WindowsSDK_IncludePath);</IncludePath>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<IncludePath>.\sourceCode;.\sourceCode\CG_frontEnd\inc;$(VC_IncludePath);$(WindowsSDK_IncludePath);</IncludePath>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>

33
camAlgoSW/sourceCode/camAlgoSW.cpp
View File

@ -10,17 +10,6 @@
#include "compute3D.h"
#include "camAlgoSW_Export.h"
typedef struct
{
uint16_t WinRdx;//窗口在图像内的起始坐标
uint16_t y; //y坐标
uint16_t Rid; //region的ID
uint8_t Flag; //bit0是overlap标志。bit1是反光信号标志
uint8_t PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标
uint8_t data[RGN_DATA_WIN_SIZE];
//double offset;
}Luma_rgnData;
#define SV_WIN_SIZE 16
#define ALIGN_RDX ((LAZER_WIN_SIZE-SV_WIN_SIZE)/2)
std::vector<Luma_rgnData> SvPickRgnRslt(
@ -280,8 +269,15 @@ void camAlgoSW(
CamPara& calibPara,
LineCalibK* lineCalibKx,
LineCalibK* lineCalibKy,
#if _ALGO_MODULE_DEBUG
std::vector<Luma_rgnData>& frontendData_debug,
std::vector<RgnPix>& centeringPnt_debug,
std::vector<RgnSubPix>& subpixPnt_debug,
std::vector<RgnSubPixCalib>& calibSubpixPnt_debug,
#endif
std::vector<Pnt3D>& resul3DPnts)
{
PickLaserResult pFrontendResult;
PickLazerWin(
1,
@ -312,15 +308,17 @@ void camAlgoSW(
frontendPara.RgnMeanETh,
&pFrontendResult
);
//将前端数据转成输入数据格式
std::vector<Luma_rgnData> frontendData = SvPickRgnRslt(
inData,
inFrmH,
inFrmW,
pFrontendResult
);
#if _ALGO_MODULE_DEBUG
frontendData_debug.insert(frontendData_debug.end(), frontendData.begin(), frontendData.end());
#endif
std::vector<RgnPix> InRgnPnt;
genTestSream(
frontendData,
@ -332,6 +330,9 @@ void camAlgoSW(
InRgnPnt,
OutCenteringPnt
);
#if _ALGO_MODULE_DEBUG
centeringPnt_debug.insert(centeringPnt_debug.end(), OutCenteringPnt.begin(), OutCenteringPnt.end());
#endif
//Convolve
std::vector<RgnPixConvolve> OutConvolvePnt;
@ -344,6 +345,9 @@ void camAlgoSW(
subpix(
OutConvolvePnt,
OutSubpixPnt);
#if _ALGO_MODULE_DEBUG
subpixPnt_debug.insert(subpixPnt_debug.end(), OutSubpixPnt.begin(), OutSubpixPnt.end());
#endif
//将subpix转成RgnSubPixCalib格式。在硬件实现时这一步骤由硬件完成
//数据格式：每行由行同步和后续的8拍构成。行同步传送x和y数据，后8拍依次传送KX0~KX7, 和KY0 ~ KY7
@ -357,6 +361,9 @@ void camAlgoSW(
inCalibData,
outSubpixCalib
);
#if _ALGO_MODULE_DEBUG
calibSubpixPnt_debug.insert(calibSubpixPnt_debug.end(), outSubpixCalib.begin(), outSubpixCalib.end());
#endif
// Compute3D
compute3D(

18
camAlgoSW/sourceCode/camAlgoSW_Export.h
View File

@ -9,6 +9,18 @@
# define SG_APISHARED_EXPORT __declspec(dllimport)
#endif
typedef struct
{
uint16_t WinRdx;//窗口在图像内的起始坐标
uint16_t y; //y坐标
uint16_t Rid; //region的ID
uint8_t Flag; //bit0是overlap标志。bit1是反光信号标志
uint8_t PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标
uint8_t data[RGN_DATA_WIN_SIZE];
//double offset;
}Luma_rgnData;
#define _ALGO_MODULE_DEBUG 1
SG_APISHARED_EXPORT void camAlgoSW(
int inFrmH,
int inFrmW,
@ -18,4 +30,10 @@ SG_APISHARED_EXPORT void camAlgoSW(
CamPara& calibPara,
LineCalibK* lineCalibKx,
LineCalibK* lineCalibKy,
#if _ALGO_MODULE_DEBUG
std::vector<Luma_rgnData>& frontendData,
std::vector<RgnPix>& centeringPnt_debug,
std::vector<RgnSubPix>& subpixPnt_debug,
std::vector<RgnSubPixCalib>& calibSubpixPnt_debug,
#endif
std::vector< Pnt3D>& resul3DPnts);

12
camAlgoSW/sourceCode/compute3D.cpp
View File

@ -62,10 +62,14 @@ void compute3D(
double u = (double)InData.x;
double v = (double)InData.y;
double z_inv = u * plane_a + v * plane_b + plane_c;
double z = 1/ z_inv;
double x = (u - u0) * z * F_inv;
double y = (v - v0) * z * F_inv;
double tmp_x = (u - u0) * F_inv;
double tmp_y = (v - v0) * F_inv;
double z_inv = plane_a * tmp_x + plane_b * tmp_y - 1;
double z = 1 / z_inv;
z = -z * plane_c;
double x = tmp_x * z ;
double y = tmp_y * z ;
outData.x = (float)x;
outData.y = (float)y;
outData.z = (float)z;

15
camAlgoSW/sourceCode/peakCentering.cpp
View File

@ -49,6 +49,7 @@ void peakCentering(
RgnPix zeroData = {0,0,0,0,0,0,0};
//ping-pong buffer
RgnPix HSyncData_d1 = {0,0,0,0,0,0,0};
ushort LazerWinX_d1 = 0;
uchar lineBuff_0[RGN_DATA_WIN_SIZE * 2];
//#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P
uchar lineBuff_1[RGN_DATA_WIN_SIZE * 2];
@ -82,9 +83,18 @@ void peakCentering(
if(0 == i) //HSync
{
evenFlag = n % 2;
if (0 == linePk.len)
pkCenter = RGN_DATA_WIN_SIZE / 2;
else
pkCenter = linePk.start + (linePk.len >> 1);
HSyncData_d1.LazerWinX = LazerWinX_d1 + pkCenter;
if (HSyncData_d1.LazerWinY == 578)
int kkk = 1;
if ((n > 0) &&( linePk.len>0))
OutCenteringPnt.push_back(HSyncData_d1);
HSyncData_d1 = InData;
LazerWinX_d1 = InData.LazerWinX - RGN_DATA_WIN_SIZE;
pkRng_S = InData.RltvRdx & 0x0f;
pkRng_S += RGN_DATA_WIN_SIZE / 2;
@ -92,11 +102,6 @@ void peakCentering(
pkRng_E = InData.RltvRdx & 0x0f;
pkRng_E += RGN_DATA_WIN_SIZE / 2;
evenFlag = n%2;
if(0 == linePk.len)
pkCenter = RGN_DATA_WIN_SIZE/2;
else
pkCenter = linePk.start + (linePk.len >> 1);
linePk.start = 0;
linePk.len = 0;
linePk.value = 0;

6
camAlgoSW/sourceCode/subPix.cpp
View File

@ -84,6 +84,8 @@ void subpix(
InData = InConvolvePnt[readPtr++];
if (InData.LazerWinY == 579)
int kkk = 1;
float fx;
if(InData.nD2 == 0)
fx = 0;
@ -91,7 +93,7 @@ void subpix(
fx = (float)InData.nD1/(float)InData.nD2;
RgnSubPix a_pnt;
if(n == 1)
if(n == 0)
a_pnt.Sync = 0b01;
else
a_pnt.Sync = 0b00;
@ -99,7 +101,7 @@ void subpix(
a_pnt.y = InData.LazerWinY;
a_pnt.rid = InData.LazerWinRid;
a_pnt.value = InData.value;
a_pnt.x = (float)InData.LazerWinX + 8.0f + fx;
a_pnt.x = (float)InData.LazerWinX + 8.0f + fx + 0.5; //+0.5: adjust pnt to pixel center
a_pnt.rsv = 0;
OutSubpixPnt.push_back(a_pnt);
}

381
camAlgoSW_test/camAlgoSW_test.cpp
View File

@ -2,10 +2,387 @@
//
#include <iostream>
#include <fstream>
#include "..\camAlgoSW\sourceCode\camAlgoSW_Export.h"
#include <opencv2/opencv.hpp>
#include <vector>
#include "..\camAlgoSW\sourceCode\algoGlobals.h"
int main()
typedef struct
{
std::cout << "Hello World!\n";
int nMin; //< 最小值
int nMax; //< 最大值
} SWdNLRange;
void saveFrontEndData(char* fileName, std::vector<Luma_rgnData>& frontendData)
{
if (0 == frontendData.size())
return;
std::ofstream TXTFile(fileName);
char TXTData[250];
snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
TXTFile << TXTData << std::endl;
for (int i = 0, i_max = (int)frontendData.size(); i < i_max; i++)
{
Luma_rgnData data = frontendData[i];
snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.WinRdx, data.y, data.Rid, data.Flag, data.PeakRltvRdx);
TXTFile << TXTData << std::endl;
for (int j = 0; j < 16; j++)
{
snprintf(TXTData, sizeof(TXTData), "%02x", data.data[j]);
TXTFile << TXTData << std::endl;
}
}
TXTFile.close();
}
void readFrontEndDataFile(char* fileName, std::vector<Luma_rgnData>& frontendData)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return;
std::getline(inputFile, linedata); //第一行
int idx = 0;
Luma_rgnData a_pk;
while (std::getline(inputFile, linedata))
{
if (idx == 0)
{
int WinRdx, y, Rid, Flag, PeakRltvRdx;
sscanf_s(linedata.c_str(), "%04x %04x %04x %01x %02x", &WinRdx, &y, &Rid, &Flag, &PeakRltvRdx);
a_pk.WinRdx = WinRdx;
a_pk.y = y;
a_pk.Rid = Rid;
a_pk.Flag = Flag;
a_pk.PeakRltvRdx = PeakRltvRdx;
idx++;
}
else
{
int value;
sscanf_s(linedata.c_str(), "%02x", &value);
a_pk.data[idx - 1] = value;
idx++;
}
if (idx == 17)
{
frontendData.push_back(a_pk);
idx = 0;
}
}
inputFile.close();
return;
}
void saveCenteringData(char* filename, std::vector<RgnPix>& centeringPnt)
{
if (centeringPnt.size() < 6)
return;
int vldSize = (int)centeringPnt.size() - 6;
if ( (vldSize % 9) != 0)
return;
int grp = vldSize / 9;
std::ofstream TXTFile(filename);
char TXTData[250];
snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
TXTFile << TXTData << std::endl;
int headOffset = 6;
for (int i = 0; i < grp; i++)
{
RgnPix data = centeringPnt[i * 9 + headOffset];
snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.LazerWinX, data.LazerWinY, data.LazerWinRid, data.LazerWinFlag, data.RltvRdx);
TXTFile << TXTData << std::endl;
for (int j = 1; j <= 8; j++)
{
data = centeringPnt[i * 9 + j + headOffset];
snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinX);
TXTFile << TXTData << std::endl;
snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinY);
TXTFile << TXTData << std::endl;
}
}
TXTFile.close();
}
void saveSubpixData(char* filename, std::vector<RgnSubPix>& subpixPnt)
{
if (subpixPnt.size() < 6)
return;
std::ofstream TXTFile(filename);
char TXTData[250];
int headOffset = 6;
for (int i = 6, i_max = (int)subpixPnt.size(); i< i_max; i++)
{
RgnSubPix data = subpixPnt[i];
snprintf(TXTData, sizeof(TXTData), "%.5f %04d rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
TXTFile << TXTData << std::endl;
}
TXTFile.close();
}
void saveCalibSubpixData(char* filename, std::vector<RgnSubPixCalib>& subpixPnt)
{
if (subpixPnt.size() < 6)
return;
std::ofstream TXTFile(filename);
char TXTData[250];
int headOffset = 6;
for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
{
RgnSubPixCalib data = subpixPnt[i];
snprintf(TXTData, sizeof(TXTData), "%.5f %.5f rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
TXTFile << TXTData << std::endl;
}
TXTFile.close();
}
void readCalibK(char* paraFilename, LineCalibK* lineCalibK)
{
std::ifstream inputFile(paraFilename);
std::string linedata;
if (inputFile.is_open() == false)
return;
int i = 0;
while (std::getline(inputFile, linedata))
{
LineCalibK a_para;
sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", &a_para.calibK[0], &a_para.calibK[1], &a_para.calibK[2], &a_para.calibK[3], &a_para.calibK[4], &a_para.calibK[5], &a_para.calibK[6], &a_para.calibK[7]);
lineCalibK[i] = a_para;
i++;
}
inputFile.close();
return;
}
CamPara readLaserPlanePara(char* filename)
{
CamPara para;
memset(&para, 0, sizeof(CamPara));
std::ifstream inputFile(filename);
std::string linedata;
if (inputFile.is_open() == false)
return para;
//读取: fx, 0, cx
if (std::getline(inputFile, linedata))
{
double tmp;
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &para.F_inv, &tmp, &para.u0);
para.F_inv = 1.0 / para.F_inv;
}
else
return para;
//读取: 0, fy, cy
if (std::getline(inputFile, linedata))
{
double tmp_1, tmp_2;
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &tmp_1, &tmp_2, &para.v0);
}
else
return para;
//跳过现行
if (!std::getline(inputFile, linedata))
return para;
if (!std::getline(inputFile, linedata))
return para;
//读取: a, b, c
if (std::getline(inputFile, linedata))
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &para.plane_a, &para.plane_b, &para.plane_c);
return para;
}
void saveLineData(char* fileName, std::vector< Pnt3D>& resul3DPnts)
{
std::ofstream sw(fileName);
sw << "LineNum:1" << std::endl;
// sw << "DataType: 0" << std::endl;
// sw << "ScanSpeed: 0" << std::endl;
// sw << "PointAdjust:1" << std::endl;
// sw << "MaxTimeStamp:0_0" << std::endl;
int ptCounter = (int)resul3DPnts.size() - 4;
sw << "Line_0_0_" << ptCounter << std::endl;
for (int i = 4; i < (int)resul3DPnts.size(); i++)
{
Pnt3D a_pt = resul3DPnts[i];
float x = (float)a_pt.x;
float y = (float)a_pt.y;
float z = (float)a_pt.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}" << std::endl;
}
sw.close();
}
#define CALIB_TEST_GROUP 3
int main()
{
std::cout << "Hello World!\n";
const char* calibDataPath[CALIB_TEST_GROUP] = {
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\", //1
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
};
const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
{1,33},{1,33},{1,10}
};
//算法参数
ProcObj frontendPara;
frontendPara.StartIdx = 0;
frontendPara.EndIdx = 0;
frontendPara.MinStgTh = 5;
frontendPara.LazerWMin = 2;
frontendPara.LazerWMax = 10;
frontendPara.RflctPixTh = 128;
frontendPara.RflctOutEna = 0;//??
frontendPara.OverlapPixTh = 32;
frontendPara.EnhanStep = 2;
frontendPara.PickEna = 1;
frontendPara.RgnFltrTh = 0;
frontendPara.RmvWkEndEna = 1;
frontendPara.RmvWkEndTh = 2;
frontendPara.RmvWkEndMultCoe = 16;
frontendPara.RmvWkEndMinLen = 3;
frontendPara.PickBFEna = 0;
frontendPara.PickBkGrnd = 0;
frontendPara.PickRLenTh = 4;
frontendPara.EnergyPickEna = 1;
frontendPara.PickEnergyType = 0;
frontendPara.RgnEnergyPLen = 32;
frontendPara.RgnMeanETh = 0;
LineCalibK* lineCalibKx = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);
LineCalibK* lineCalibKy = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);
FramePara inFramePara;
CamPara calibPara;
for (int grp = 0; grp < CALIB_TEST_GROUP; grp++)
{
memset(lineCalibKx, 0, sizeof(LineCalibK) * MAX_HEIGHT);
memset(lineCalibKy, 0, sizeof(LineCalibK) * MAX_HEIGHT);
//读取校正系数
char paraFilename[256];
sprintf_s(paraFilename, "%scalib_param_x.txt", calibDataPath[grp]);
readCalibK(paraFilename, lineCalibKx);
sprintf_s(paraFilename, "%scalib_param_y.txt", calibDataPath[grp]);
readCalibK(paraFilename, lineCalibKy);
//读取光刀面
sprintf_s(paraFilename, "%scalib_param_K_D.txt", calibDataPath[grp]);
calibPara = readLaserPlanePara(paraFilename);
int startIndex = fileIdx[grp].nMin;
int endIndex = fileIdx[grp].nMax;
int index;
for (index = startIndex; index <= endIndex; index++)
{
char filename[256];
sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename);
if (srcImg.empty())
break;
inFramePara.FrmNo = index;
inFramePara.timeStamp = index;
inFramePara.encInfo = 0;
inFramePara.frameROI_w = srcImg.cols;
inFramePara.frameROI_h = srcImg.rows;
inFramePara.frameROI_x = 0;
inFramePara.frameROI_y = 0;
cv::Mat gray;
if (srcImg.channels() == 3)
cv::cvtColor(srcImg, gray, cv::COLOR_BGR2GRAY);
else
gray = srcImg.clone();
unsigned char* dataPtr = gray.data;
std::vector< Pnt3D> resul3DPnts;
#if _ALGO_MODULE_DEBUG
std::vector<Luma_rgnData> frontendData;
std::vector<RgnPix> centeringPnt;
std::vector<RgnSubPix> subpixPnt;
std::vector<RgnSubPixCalib> calibSubpixPnt;
#endif
camAlgoSW(
gray.rows,
gray.cols,
dataPtr,
frontendPara,
inFramePara,
calibPara,
lineCalibKx,
lineCalibKy,
#if _ALGO_MODULE_DEBUG
frontendData,
centeringPnt,
subpixPnt,
calibSubpixPnt,
#endif
resul3DPnts);
#if _ALGO_MODULE_DEBUG
//保存frontendData
sprintf_s(filename, "%sresult\\%d_verilog.txt", calibDataPath[grp], index);
saveFrontEndData(filename, frontendData);
//保存centeringData
sprintf_s(filename, "%sresult\\%d_centeringData.txt", calibDataPath[grp], index);
saveCenteringData(filename, centeringPnt);
//保存subpixData
sprintf_s(filename, "%sresult\\%d_subpixData.txt", calibDataPath[grp], index);
saveSubpixData(filename, subpixPnt);
//显示亚像素点
cv::Mat enlargeImg;
if (srcImg.channels() == 1)
srcImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
else
enlargeImg = srcImg.clone();
cv::Size objSize = srcImg.size();
objSize.width = objSize.width * 5;
cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
{
RgnSubPix a_subPix = subpixPnt[i];
int row = (int)(a_subPix.y + 0.5);
int col = (int)(a_subPix.x * 5 + 0.5);
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
}
sprintf_s(filename, "%sresult\\%d_subpix.png", calibDataPath[grp], index);
cv::imwrite(filename, enlargeImg);
//保存校正后的subpixData
sprintf_s(filename, "%sresult\\%d_calibSubpixData.txt", calibDataPath[grp], index);
saveCalibSubpixData(filename, calibSubpixPnt);
#endif
sprintf_s(filename, "%sresult\\%d_3D_data.txt", calibDataPath[grp], index);
saveLineData(filename, resul3DPnts);
}
}
}

11
camAlgoSW_test/camAlgoSW_test.vcxproj
View File

@ -78,11 +78,13 @@
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<IncludePath>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(IncludePath)</IncludePath>
<IncludePath>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(IncludePath);F:\ShangGu\ProductDev\三角光相机\相机开发\camAlgo_git\camAlgoSW\sourceCode</IncludePath>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<IncludePath>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(VC_IncludePath);$(WindowsSDK_IncludePath);</IncludePath>
<IncludePath>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(VC_IncludePath);$(WindowsSDK_IncludePath);;F:\ShangGu\ProductDev\三角光相机\相机开发\camAlgo_git\camAlgoSW\sourceCode</IncludePath>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
@ -122,6 +124,8 @@
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;.\sourceCode\CG_frontEnd\lib\debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world480d.lib;camAlgoSW.lib;ImgAccd.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
@ -132,12 +136,15 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<Optimization>Disabled</Optimization>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;F:\ShangGu\ProductDev\三角光相机\相机开发\camAlgo_git\camAlgoSW\sourceCode\CG_frontEnd\lib\debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world480.lib;camAlgoSW.lib;ImgAccd.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemGroup>

322
camCalib/camCalib.cpp
View File

@ -29,7 +29,7 @@ void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
return;
}
void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe)
{
std::ofstream sw(fileName);
@ -50,6 +50,20 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
temp[_c] = D.ptr<double>(0)[_c];
sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]);
sw << dataStr << std::endl;
//ax+by+cz+d = 0
float a = (float)pe[0];
float b = (float)pe[1];
float c = (float)pe[2];
float d = (float)pe[3];
//将c变成-1，转成z=ax+by+c的形式，使用3个参数
a = -a / c;
b = -b / c;
d = -d / c;
c = -1;
sprintf_s(dataStr, 250, "%g, %g, %g", a, b, d);
sw << dataStr << std::endl;
sw.close();
return;
}
@ -87,6 +101,24 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
return;
}
void saveSubpixData(char* filename, std::vector<cv::Point2f>& subpixPnt)
{
if (subpixPnt.size() < 6)
return;
std::ofstream TXTFile(filename);
char TXTData[250];
int headOffset = 6;
for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
{
cv::Point2f a_subPix = subpixPnt[i];
snprintf(TXTData, sizeof(TXTData), "%.5f %.5f", a_subPix.x, a_subPix.y);
TXTFile << TXTData << std::endl;
}
TXTFile.close();
}
typedef struct
{
int nMin; //< 最小值
@ -130,13 +162,13 @@ int main()
std::cout << "Hello World!\n";
const char* calibDataPath[CALIB_TEST_GROUP] = {
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\chessboard\\", //1
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\", //0
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\", //1
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
};
const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
{3,39},{1,200},{1,166},{122,141}
{3,39},{1,33},{1,33},{1,10}
};
const int boardType[CALIB_TEST_GROUP] =
{
@ -148,7 +180,7 @@ int main()
for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
{
grp = 2;
grp = 1;
int calibType = boardType[grp];
cv::Size cbPattern;
float cbSquareSize;
@ -283,9 +315,7 @@ int main()
std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;
//生成矫正图像
index = 3;
for (;; index++) {
for (index = startIndex; index <= endIndex; index++) {
char filename[256];
sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename);
@ -306,10 +336,6 @@ int main()
cv::imwrite(filename, calibImg);
}
//output K and D
char calibKDName[256];
sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
sg_outputCalibKD(calibKDName, K, D);
#else
char calibKDName[256];
sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath);
@ -354,136 +380,156 @@ int main()
#endif
{
std::vector<cv::Point3f> all_pts3d;
std::vector<cv::Point3f> all_pts3d;
for (index = startIndex; index <= endIndex; index++) {
index = 3;
for (;; index++) {
char filename[256];
sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename);
if (srcImg.empty())
break;
char filename[256];
sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcImg = cv::imread(filename);
if (srcImg.empty())
break;
cv::Mat img;
cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
std::vector<cv::Point2f> corners;
detectCorners(img, cbPattern, corners);
if (corners.empty())
continue;
cv::Mat img;
cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
std::vector<cv::Point2f> corners;
detectCorners(img, cbPattern, corners);
if (corners.empty())
continue;
// 创建棋盘格区域的掩码
cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
// 使用多边形近似来填充角点之间的区域
// 棋盘格区域需要比角点区域大一圈
std::vector<cv::Point2f> contour_line[4];
for (int i = 0; i < cbPattern.width; i++)
{
cv::Point2f pt_c = corners[i];
cv::Point2f pt_2 = corners[cbPattern.width + i];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[0].push_back(pt_1);
}
for (int i = 0; i < cbPattern.height; i++)
{
cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[1].push_back(pt_1);
}
for (int i = cbPattern.width - 1; i >= 0; i--)
{
cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[2].push_back(pt_1);
}
for (int i = cbPattern.height-1; i >= 0; i--)
{
cv::Point2f pt_c = corners[i * cbPattern.width];
cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[3].push_back(pt_1);
}
std::vector<cv::Point> contours;
//生成轮廓点
for (int n = 0; n < 4; n++)
{
int num = contour_line[n].size();
for (int i = 0; i < num; i++)
contours.push_back(contour_line[n][i]);
cv::Point2f pt_c = contour_line[n][num - 1];
cv::Point2f pt_2 = contour_line[n][num - 2];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contours.push_back(pt_1);
}
// 使用 fillPoly 填充多边形
cv::Scalar color(255); // 红色
cv::fillPoly(chessMask, contours, color);
#if 1
sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, chessMask);
#endif
cv::Vec4f pe;
fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcLaserImg = cv::imread(filename);
if (srcLaserImg.empty())
break;
cv::Mat laserImg_unMask;
cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
//与Mask相与，保证待处理的激光线在标定板上
cv::Mat laserImg;
cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
#if 1
sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, laserImg);
#endif
std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
//显示亚像素点
cv::Mat enlargeImg;
if (laserImg.channels() == 1)
laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
else
enlargeImg = laserImg.clone();
cv::Size objSize = laserImg.size();
objSize.width = objSize.width * 5;
cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
for (int i = 0, i_max = pts2d.size(); i < i_max; i++)
{
cv::Point2f a_subPix = pts2d[i];
a_subPix.x += 0.5;
int row = (int)(a_subPix.y + 0.5);
int col = (int)(a_subPix.x * 5 + 0.5);
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
}
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
cv::imwrite(filename, enlargeImg);
std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
// 创建棋盘格区域的掩码
cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
// 使用多边形近似来填充角点之间的区域
// 棋盘格区域需要比角点区域大一圈
std::vector<cv::Point2f> contour_line[4];
for (int i = 0; i < cbPattern.width; i++)
{
cv::Point2f pt_c = corners[i];
cv::Point2f pt_2 = corners[cbPattern.width + i];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[0].push_back(pt_1);
}
for (int i = 0; i < cbPattern.height; i++)
{
cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[1].push_back(pt_1);
}
for (int i = cbPattern.width - 1; i >= 0; i--)
{
cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[2].push_back(pt_1);
}
for (int i = cbPattern.height-1; i >= 0; i--)
{
cv::Point2f pt_c = corners[i * cbPattern.width];
cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contour_line[3].push_back(pt_1);
}
std::vector<cv::Point> contours;
//生成轮廓点
for (int n = 0; n < 4; n++)
{
int num = contour_line[n].size();
for (int i = 0; i < num; i++)
contours.push_back(contour_line[n][i]);
cv::Point2f pt_c = contour_line[n][num - 1];
cv::Point2f pt_2 = contour_line[n][num - 2];
cv::Point2f pt_1;
pt_1.x = pt_c.x * 2 - pt_2.x;
pt_1.y = pt_c.y * 2 - pt_2.y;
contours.push_back(pt_1);
}
// 使用 fillPoly 填充多边形
cv::Scalar color(255); // 红色
cv::fillPoly(chessMask, contours, color);
#if 1
sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, chessMask);
#endif
cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
std::cout << "pe: " << pe << std::endl;
cv::Vec4f pe;
fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
cv::Mat srcLaserImg = cv::imread(filename);
if (srcLaserImg.empty())
break;
cv::Mat laserImg_unMask;
cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
//与Mask相与，保证待处理的激光线在标定板上
cv::Mat laserImg;
cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
#if 1
sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
cv::imwrite(filename, laserImg);
cv::Mat calibImg;
remap(laserImg,
calibImg,
mapGen_x,
mapGen_y,
cv::INTER_LINEAR,
cv::BORDER_CONSTANT,
cv::Scalar(0, 0, 0));
sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
cv::imwrite(filename, calibImg);
#endif
std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
//显示亚像素点
cv::Mat enlargeImg;
if (laserImg.channels() == 1)
laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
else
enlargeImg = laserImg.clone();
cv::Size objSize = laserImg.size();
objSize.width = objSize.width * 5;
cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
if (pts2d.size() > 0)
{
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index);
saveSubpixData(filename, pts2d);
}
for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++)
{
cv::Point2f a_subPix = pts2d[i];
int row = (int)(a_subPix.y + 0.5);
int col = (int)(a_subPix.x * 5 + 0.5);
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
}
sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
cv::imwrite(filename, enlargeImg);
std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
#if 1
//保存3D点
#endif
all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
}
cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
std::cout << "pe: " << pe << std::endl;
//output K and D
char calibKDName[256];
sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
sg_outputCalibKD(calibKDName, K, D, pe);
}
return 0;

2
camCalib/lineDetection_steger.cpp
View File

@ -436,7 +436,7 @@ void computePointSubpix(
if ((subpix.x >= 0) && (subpix.y >= 0))
{
subpix.x += 0.5; //图像中的像素位置表示的是像素是左下角，而此处像素坐标是指向像素中间位置，所以有0.5像素差
subpix.x -= 0.5; //此处图像的Y坐标系方向与通常坐标系的方向相反
subpix.y -= 0.5; //此处图像的Y坐标系方向与通常坐标系的方向相反
posSubpix.push_back(subpix);
doSubPix = -1;
}

2
camCalib/sourceCode/MonoLaserCalibrate.cpp
View File

@ -191,7 +191,7 @@ void monocularCalibration(
flags |= cv::fisheye::CALIB_FIX_SKEW;
cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);// , flags, cv::TermCriteria(3, 20, 1e-6));
#else
cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);
cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, cv::CALIB_FIX_ASPECT_RATIO);
#endif
// 重投影三维点到二维图像点
// 计算重投影误差