-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.cpp
123 lines (99 loc) · 5.42 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include "./include/Common/cmAlgo.h"
#include "./include/StereoMapping/CostCalculator/smCostCalculator.h"
#include "./include/StereoMapping/CostCalculator/smCensusTransformCostCalculator.h"
#include "./include/StereoMapping/CostCalculator/smFourPathCostAggregator.h"
#include "./include/StereoMapping/CostCalculator/smEightPathCostAggregator.h"
#include "./include/StereoMapping/CostOptimizer/smCostOptimizer.h"
#include "./include/StereoMapping/Helper/smCostHelper.h"
#include "./include/StereoMapping/DepthEstimator/smDepthConverter.h"
#include "./include/DenseReconstruction/Voxel/drPlainVoxelStore.h"
#include "./include/DenseReconstruction/TSDF/drTSDF.h"
#include "./include/Common/Utility/cmVisExt.h"
#define CE_TYPE u32
#define SGM_ONLY true
using namespace std;
using namespace cv;
int main() {
cout << "Starting" << endl;
//Create Objects
StereoMapping::CostHelper helper = StereoMapping::CostHelper();
StereoMapping::CostOptimizer optimizer = StereoMapping::CostOptimizer();
StereoMapping::DepthConverter depthConverter = StereoMapping::DepthConverter();
DenseReconstruction::TruncatedSDF tsdfCalc = DenseReconstruction::TruncatedSDF();
Common::Util::VisualizationExt visExt = Common::Util::VisualizationExt();
//Load Images
cout << "Loading Image" << endl;
cv::Mat imageLeft = cv::imread("C:/WR/Dense-Reconstruction/samples/vl.png", 0);
cv::Mat imageRight = cv::imread("C:/WR/Dense-Reconstruction/samples/vr.png", 0);
u32 imageWidth = imageLeft.size[1];
u32 imageHeight = imageLeft.size[0];
u32 disparityRange = 64;
cout << "IW=" << imageWidth << ", IH=" << imageHeight << endl;
//Disparity Estimate
cout << "Estimating Disparity" << endl;
f64* leftDisparityMap = allocate_mem(f64, imageWidth * imageHeight);
f64* leftDisparityMapS = allocate_mem(f64, imageWidth * imageHeight);
u32* occlusionList = allocate_mem(u32, imageWidth * imageHeight);
u32* mismatchList = allocate_mem(u32, imageWidth * imageHeight);
u32 occlusionListLen = 0, mismatchListLen = 0;
helper.calculateCostInternalF(imageLeft.data, imageRight.data, imageWidth, imageHeight, disparityRange, leftDisparityMap, leftDisparityMapS, occlusionList, &occlusionListLen, mismatchList, &mismatchListLen);
//Peak Check
cout << "Peak Check" << endl;
f64* leftDisparityMapCbc = allocate_mem(f64, imageWidth * imageHeight);
optimizer.smConnectedBlockFiltering(leftDisparityMap, leftDisparityMapCbc, imageWidth, imageHeight, 1.1, 10);
//Disparity Fill
cout << "Disparity Fill" << endl;
f64* leftDisparityMapFl = allocate_mem(f64, imageWidth * imageHeight);
optimizer.smDisparityFill(leftDisparityMapCbc, leftDisparityMapFl, imageWidth, imageHeight, occlusionList, &occlusionListLen, mismatchList, &mismatchListLen);
//Median Filter
cout << "Median Filter" << endl;
f64* leftDisparityMapMf = allocate_mem(f64, imageWidth * imageHeight);
optimizer.smMedianFilter(leftDisparityMapFl, leftDisparityMapMf, imageWidth, imageHeight, 3);
cout << "Desc" << endl;
u32* leftDisparityMapMfds = allocate_mem(u32, imageWidth * imageHeight);
optimizer.smDisparityMapDiscretization(leftDisparityMapCbc, leftDisparityMapMfds, imageWidth, imageHeight, disparityRange, 999.0);
cout << "Saving PPM" << endl;
Common::Algorithm::cmSaveAsPPM32("C:/WR/Dense-Reconstruction/samples/vs1-cb-3da.ppm", leftDisparityMapMfds, imageWidth, imageHeight, disparityRange);
if (!SGM_ONLY) {
//=========== End of Disparity Estimation ==================
cout << "Depth Estimation" << endl;
f64* depthMap = allocate_mem(f64, imageWidth * imageHeight);
depthConverter.smIdealBinocularDisparityToDepth(leftDisparityMapMf, depthMap, imageWidth, imageHeight, 20.0, 60.0);
//Discretization
cout << "Discretization" << endl;
u32* depthMapTrunc = allocate_mem(u32, imageWidth * imageHeight);
u32 depthMapTruncMax = 0;
depthConverter.smDepthDiscretization(depthMap, depthMapTrunc, &depthMapTruncMax, imageWidth, imageHeight);
//=========== End of Depth Calculation ==================
cout << "Initializing Camera Intrinsic" << endl;
Common::MonocularCameraIntrinsic* cameraIntrinsic = new Common::MonocularCameraIntrinsic();
cameraIntrinsic->fx = 450.0;
cameraIntrinsic->fy = 375.0;
cameraIntrinsic->cx = 450.0 / 2.0;
cameraIntrinsic->cy = 375.0 / 2.0;
cameraIntrinsic->dx = 1.0;
cameraIntrinsic->dy = 1.0;
cout << "Creating Voxels" << endl;
DenseReconstruction::VoxelStore* voxelStore = new DenseReconstruction::PlainVoxelStore();
voxelStore->drInitialize(128, 128, 128, -64, -64, -64, 1.0);
DenseReconstruction::VoxelStore* voxelStoreTemp = new DenseReconstruction::PlainVoxelStore();
voxelStoreTemp->drInitialize(128, 128, 128, -64, -64, -64, 1.0);
cout << "Calculating Truncated Signed Distance Field" << endl;
tsdfCalc.drIdealFirstTsdfEstimate(depthMap, imageWidth, imageHeight, cameraIntrinsic, voxelStore, 10.0);
cout << "Converting TSDF" << endl;
visExt.cmuTsdfBinarization(voxelStore, voxelStoreTemp);
cout << "Converting to Mesh" << endl;
Common::Mesh::SimpleMesh* mesh = new Common::Mesh::SimpleMesh();
visExt.cmuVoxelMarchingCubes(voxelStore, mesh);
cout << "F=" << mesh->f.size() << ", V=" << mesh->v.size() << endl;
cout << "Saving As OBJ" << endl;
visExt.cmuExportMeshToObj("C:/WR/Sayu/samples/3d-3c.obj", mesh);
//Save PPM
cout << "Saving PPM" << endl;
Common::Algorithm::cmSaveAsPPM32("C:/WR/Dense-Reconstruction/samples/vs1-cb-3c.ppm", depthMapTrunc, imageWidth, imageHeight, depthMapTruncMax);
}
return 0;
}