Skip to content

Commit 4ff95d8

Browse files
arczeearczee
authored
Add files via upload
1 parent b948fac commit 4ff95d8

17 files changed

+3188
-0
lines changed

‎src/CMakeLists.txt‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
project(autodiff)
2+
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
3+
AUX_SOURCE_DIRECTORY(. DIR_SRCS)
4+
5+
#add_subdirectory(CL_files)
6+
#add_subdirectory(src)
7+
8+
#################指定OpenCL库依赖, 手动指定方式#######################
9+
#指定Opencl的目录
10+
SET(OPENCL_INCLUDE_PATH /usr/include CACHE PATH "Path to OpenCL include files")
11+
SET(OPENCL_LIB_PATH /usr/lib CACHE PATH "Path to OpenCL libraries")
12+
include_directories(${OPENCL_INCLUDE_PATH}) #指定OpenCL头文件的搜索目录
13+
link_directories(${OPENCL_LIB_PATH}) #指定OpenCL库的搜索目录
14+
15+
################指定OpenCL库依赖, find_package方式#######################
16+
#find_package(opencl)
17+
#include_directories(${OpenCL_INCLUDE_DIR})
18+
#LINK_DIRECTORIES(${OpenCL_LIBRARY})
19+
#TARGET_LINK_LIBRARIES(PSBA OpenCL) #实际会形成OpenCL.lib库依赖
20+
21+
22+
#放在最后
23+
ADD_EXECUTABLE(autodiff ${DIR_SRCS}) #最终生成的可执行命令
24+
TARGET_LINK_LIBRARIES(autodiff OpenCL) #实际会形成OpenCL.lib库依赖

‎src/SfMdata.cpp‎

Lines changed: 323 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,323 @@
1+
2+
#include "SfMdata.h"
3+
4+
SfMdata::~SfMdata() {
5+
if (nCams_!=0)
6+
{
7+
delete[]Kdata; delete[] Poses;
8+
//delete[]initRot;
9+
}
10+
11+
if (initialized)
12+
{
13+
delete []ji_idx; delete []DijL1Blocks;
14+
delete[]posPoC; delete[]posCoP; delete[]PoCidxs; delete[]CoPidxs;
15+
delete []pts2D; delete [] pts3D;
16+
}
17+
//清除DijL2BlockList中分配的内存
18+
vector<int*>::iterator iter = DijL2BlockList.begin();
19+
for (iter = DijL2BlockList.begin(); iter != DijL2BlockList.end(); iter++) {
20+
delete [](*iter);
21+
}
22+
iter = DijL3BlockList.begin();
23+
for (iter = DijL3BlockList.begin(); iter != DijL3BlockList.end(); iter++) {
24+
delete[](*iter);
25+
}
26+
}
27+
28+
int SfMdata::initCams(int nCams, int nRotPara)
29+
{
30+
if (this->nCams_ != 0) return -1;
31+
this->nCams_ = nCams;
32+
33+
Kdata = new double[nCams * 5];
34+
//initRot = new double[nCams * nRotPara];
35+
Poses = new double[nCams*(nRotPara-1 + 3)];
36+
37+
return 0;
38+
}
39+
40+
41+
//初始化存储空间
42+
int SfMdata::init(int nPts3D, int nPts2D, int nPOC, int nCOP, int*posPoC, int* posCoP) {
43+
44+
if (nCams_ == 0 || initialized) return -1; //先要读取相机参数
45+
46+
this->nPts2D_ = nPts2D;
47+
this->nPts3D_ = nPts3D;
48+
this->nPoC_ = nPOC;
49+
this->nCoP_ = nCOP;
50+
51+
//确定L1_BLOCK_SIZE和L2_BLOCK_SIZE的尺寸
52+
L3_BLOCK_SIZE = 250;
53+
L2_BLOCK_SIZE = 250; //二级索引BLOCK默认为250
54+
L1_BLOCK_SIZE = nPts3D / (L2_BLOCK_SIZE*L3_BLOCK_SIZE)+1;
55+
//L1_BLOCK_SIZE = 1024;
56+
57+
58+
pts3D = new double[nPts3D * 3];
59+
pts2D = new double[nPts2D * 2];
60+
ji_idx = new int[nPts2D]; memset(ji_idx, NON_EXIST_FLAG, nPts2D*2 * sizeof(int));
61+
//jidx = new int[nPts2D]; memset(jidx, NON_EXIST_FLAG, nPts2D * sizeof(int));
62+
63+
this->posPoC = new int[nCams_+1]; memcpy(this->posPoC, posPoC, (nCams_+1) * sizeof(int));
64+
this->posCoP = new int[nPts3D+1]; memcpy(this->posCoP, posCoP, (nPts3D+1) * sizeof(int));
65+
this->cntPoC = new int[nCams_]; memset(this->cntPoC, 0, nCams_ * sizeof(int));
66+
this->cntCoP = new int[nPts3D]; memset(this->cntCoP, 0, nPts3D * sizeof(int));
67+
PoCidxs = new int[nPOC];
68+
PoCidxs2 = new int[nPOC];
69+
CoPidxs = new int[nCOP];
70+
CoPidxs2 = new int[nCOP];
71+
72+
DijL1Blocks = new int[nCams_ * L1_BLOCK_SIZE * sizeof(int)];
73+
memset(DijL1Blocks, NON_EXIST_FLAG, nCams_ * L1_BLOCK_SIZE * sizeof(int));
74+
75+
initialized = true;
76+
return 0;
77+
}
78+
79+
void SfMdata::finishRead() {
80+
if (initialized) {
81+
delete[] cntPoC; delete[]cntCoP;
82+
}
83+
readFinished = true;
84+
}
85+
86+
87+
/*
88+
1, 将3D点注册到所属的相机(PoCidxs)中. 2, 将相机注册到所属的3D点(CoPidxs)中
89+
pos为2D点索引
90+
*/
91+
int SfMdata::registerPOCandCOP(int i, int j, int idx2D)
92+
{
93+
int pos;
94+
pos = posPoC[j] + cntPoC[j];
95+
PoCidxs[pos] = i;
96+
PoCidxs2[pos] = idx2D;
97+
cntPoC[j]++;
98+
99+
pos = posCoP[i] + cntCoP[i];
100+
CoPidxs[pos] = j;
101+
CoPidxs2[pos] = idx2D;
102+
cntCoP[i]++;
103+
104+
return 0;
105+
}
106+
107+
108+
//注册Dij的位置，便于快速查找
109+
/*
110+
=====返回值=====
111+
==-1, 内存分配错误 ==0 正确
112+
*/
113+
int SfMdata::registerDijPos(int i, int j, int DijPos)
114+
{
115+
int L23_SIZE;
116+
int L1pos,L2pos,L3pos, L2idx,L3idx, TT;
117+
int *ptr;
118+
L23_SIZE = L2_BLOCK_SIZE*L3_BLOCK_SIZE;
119+
L1pos = i / L23_SIZE; //确定3D点i的L2区间在L1中的位置
120+
TT = i - L1pos*L23_SIZE;
121+
L2pos = TT / L3_BLOCK_SIZE;
122+
L3pos = TT - L2pos*L3_BLOCK_SIZE;
123+
124+
if (L1pos < 0 || L1pos >= L1_BLOCK_SIZE) return 1;
125+
if (L2pos < 0 || L2pos >= L2_BLOCK_SIZE) return 1;
126+
if (L3pos < 0 || L3pos >= L3_BLOCK_SIZE) return 1;
127+
//确定L2idx
128+
L2idx = DijL1Blocks[j*L1_BLOCK_SIZE + L1pos];
129+
if (L2idx == NON_EXIST_FLAG) //表示该区间还没有分配二级索引块
130+
{
131+
//申请一块内存，存入DijL2BlockList
132+
ptr = new int[L2_BLOCK_SIZE]; memset(ptr, NON_EXIST_FLAG, L2_BLOCK_SIZE * sizeof(int));
133+
if (ptr == NULL) return -1;
134+
DijL2BlockList.push_back(ptr);
135+
L2idx= (DijL2BlockList.size() - 1);
136+
DijL1Blocks[j*L1_BLOCK_SIZE + L1pos] = L2idx;
137+
}
138+
//确定L3idx
139+
L3idx = DijL2BlockList[L2idx][L2pos];
140+
if (L3idx == NON_EXIST_FLAG)
141+
{
142+
//申请一块内存，存入DijL2BlockList
143+
ptr = new int[L3_BLOCK_SIZE]; memset(ptr, NON_EXIST_FLAG, L3_BLOCK_SIZE * sizeof(int));
144+
if (ptr == NULL) return -1;
145+
DijL3BlockList.push_back(ptr);
146+
L3idx = (DijL3BlockList.size() - 1);
147+
DijL2BlockList[L2idx][L2pos] = L3idx;
148+
}
149+
DijL3BlockList[L3idx][L3pos] = DijPos;
150+
151+
return 0;
152+
}
153+
154+
/*
155+
=====返回值=====
156+
>=0 正常的Dij索引位置， <0 没有索引（即Dij的内容为零） 或错误
157+
158+
*/
159+
int SfMdata::getDijPos(int i, int j)
160+
{
161+
int L23_SIZE;
162+
int L1pos, L2pos, L3pos, L2idx, L3idx, TT;
163+
int *ptr;
164+
L23_SIZE = L2_BLOCK_SIZE*L3_BLOCK_SIZE;
165+
L1pos = i / L23_SIZE; //确定3D点i的L2区间在L1中的位置
166+
TT = i - L1pos*L23_SIZE;
167+
L2pos = TT / L3_BLOCK_SIZE;
168+
L3pos = TT - L2pos*L3_BLOCK_SIZE;
169+
170+
if (L1pos < 0 || L1pos >= L1_BLOCK_SIZE) return 1;
171+
if (L2pos < 0 || L2pos >= L2_BLOCK_SIZE) return 1;
172+
if (L3pos < 0 || L3pos >= L3_BLOCK_SIZE) return 1;
173+
174+
L2idx=DijL1Blocks[j*L1_BLOCK_SIZE + L1pos];
175+
if ( L2idx == NON_EXIST_FLAG) return -2;
176+
L3idx = DijL2BlockList[L2idx][L2pos];
177+
if (L3idx == NON_EXIST_FLAG) return -2;
178+
return DijL3BlockList[L3idx][L3pos];
179+
}
180+
181+
void SfMdata::display_idxs()
182+
{
183+
int *ptr;
184+
185+
cout << "============ij_idx============" << endl;
186+
for (int i = 0; i < nPts2D_; i++)
187+
cout <<"["<< ji_idx[2*i] << " "<< ji_idx[2 * i+1]<<"[";
188+
cout << endl;
189+
190+
///*
191+
cout << "============Dij============" << endl;
192+
cout << "======L1======" << endl;
193+
for (int i = 0; i < nCams_; i++) {
194+
cout << "[j " << i << "]: ";
195+
for (int j = 0; j < L1_BLOCK_SIZE; j++)
196+
cout << DijL1Blocks[i*L1_BLOCK_SIZE + j] << " ";
197+
cout << endl;
198+
}
199+
cout << "======L2======" << endl;
200+
for (int idx = 0; idx < DijL2BlockList.size(); idx++) {
201+
cout << "[Idx " << idx <<"]: ";
202+
ptr = DijL2BlockList[idx];
203+
for (int k = 0; k < L2_BLOCK_SIZE; k++)
204+
cout << ptr[k] << " ";
205+
cout << endl;
206+
}
207+
208+
cout << "======L3======" << endl;
209+
for (int idx = 0; idx < DijL3BlockList.size(); idx++) {
210+
cout << "[Idx " << idx << "]: ";
211+
ptr = DijL3BlockList[idx];
212+
for (int k = 0; k < L3_BLOCK_SIZE; k++)
213+
cout << ptr[k] << " ";
214+
cout << endl;
215+
}
216+
//*/
217+
218+
cout << "============POC============" << endl;
219+
cout << "======posPoC======" << endl;
220+
for (int i = 0; i < (nCams_+1); i++)
221+
cout << posPoC[i] << " ";
222+
cout << endl;
223+
cout << "======PoCidxs======" << endl;
224+
cout << "size:" << nPoC_ << endl;
225+
for (int i = 0; i < nPoC_; i++)
226+
cout << PoCidxs[i] << " ";
227+
cout << endl;
228+
cout << "======PoCidxs2======" << endl;
229+
cout << "size:" << nPoC_ << endl;
230+
for (int i = 0; i < nPoC_; i++)
231+
cout << PoCidxs2[i] << " ";
232+
cout << endl;
233+
cout << "============COP============" << endl;
234+
cout << "======posCoP======" << endl;
235+
for (int i = 0; i < (nPts3D_+1); i++)
236+
cout << posCoP[i] << " ";
237+
cout << endl;
238+
cout << "======CoPidxs======" << endl;
239+
cout << "size:" << nCoP_ << endl;
240+
for (int i = 0; i < nCoP_; i++)
241+
cout << CoPidxs[i] << " ";
242+
cout << endl;
243+
cout << "======CoPidxs2======" << endl;
244+
cout << "size:" << nCoP_ << endl;
245+
for (int i = 0; i < nCoP_; i++)
246+
cout << CoPidxs2[i] << " ";
247+
cout << endl;
248+
}
249+
250+
void SfMdata::display_pts(int firstN2D, int firstN3D)
251+
{
252+
cout << "=====2D points=====" << endl;
253+
for (int i = 0; i < firstN2D; i++) {
254+
printf("[%d,%d]:(%le,%le) ", ji_idx[i*2],ji_idx[i*2+1], pts2D[i * 2], pts2D[i * 2 + 1]);
255+
if ((i+1) % 5 == 0) cout << endl;
256+
}
257+
cout << endl;
258+
cout << "=====3D points=====" << endl;
259+
for (int i = 0; i < firstN3D; i++) {
260+
printf("[%d]:(%le,%le,%le) ", i, pts3D[i * 3], pts3D[i * 3 + 1], pts3D[i * 3 + 2]);
261+
if ((i+1) % 5 == 0) cout << endl;
262+
}
263+
cout << endl;
264+
}
265+
266+
void SfMdata::display_info()
267+
{
268+
cout << "============Info============" << endl;
269+
cout << "Num. of Cams: " << nCams_ << endl;
270+
cout << "Num. of Pts 3D: " << nPts3D_ << endl;
271+
cout << "Num. of Pts 2D: " << nPts2D_ << endl;
272+
cout << "L1_BLOCK_SIZE: " <<L1_BLOCK_SIZE<< endl;
273+
cout << "L2_BLOCK_SIZE: " << L2_BLOCK_SIZE << endl;
274+
cout << "L3_BLOCK_SIZE: " << L3_BLOCK_SIZE << endl;
275+
cout << "nDijL1Blocks:" << nCams_ << endl;
276+
cout << "nDijL2Blocks:" << DijL2BlockList.size() << endl;
277+
cout << "nDijL3Blocks:" << DijL3BlockList.size() << endl;
278+
cout << "Num. of POC: " << nPoC_ << endl;
279+
cout << "Num. of COP: " << nCoP_ << endl;
280+
281+
}
282+
283+
void SfMdata::display_cameras(int firstNcams)
284+
{
285+
double *ptr;
286+
cout << "============Cameras============" << endl;
287+
cout << "=====K parameters=====" << endl;
288+
for (int i = 0; i < firstNcams; i++)
289+
{
290+
//cout << "==Camera " << i << "==" << endl;
291+
ptr = (Kdata + i * 5);
292+
printf("[%d]: %.8e,%.8e,%.8e,%.8e,%.8e\n", i, *ptr, *(ptr + 1), *(ptr + 2), *(ptr + 3), *(ptr + 4));
293+
}
294+
/*
295+
cout << "=====InitRot=====" << endl;
296+
for (int i = 0; i < nCams_; i++)
297+
{
298+
ptr = (initRot + i * 4);
299+
printf("[%d]: %le,%le,%le,%le\n", i, *ptr, *(ptr + 1), *(ptr + 2), *(ptr + 3));
300+
}*/
301+
cout << "=====Poses=====" << endl;
302+
for (int i = 0; i < firstNcams; i++)
303+
{
304+
ptr = (Poses + i *6);
305+
printf("[%d]: %.8e,%.8e,%.8e,%.8e,%.8e,%.8e\n",
306+
i, *ptr, *(ptr + 1), *(ptr + 2), *(ptr + 3), *(ptr + 4), *(ptr + 5));
307+
}
308+
}
309+
310+
311+
312+
void SfMdata::test_DijLx()
313+
{
314+
bool Dij_passed = true;
315+
int k;
316+
for (k = 0; k < nPts2D_; k++) {
317+
int i = ji_idx[2*k]; int j= ji_idx[2*k+1];
318+
if (getDijPos(i, j) != k)
319+
Dij_passed = false;
320+
}
321+
if (Dij_passed) cout << "Dij indexing passed." << endl;
322+
else cout << "Dij indexing failed.";
323+
}

0 commit comments

Comments
 (0)