-
Notifications
You must be signed in to change notification settings - Fork 23
Expand file tree
/
Copy pathpointcloudimage.cpp
More file actions
164 lines (149 loc) · 4.56 KB
/
pointcloudimage.cpp
File metadata and controls
164 lines (149 loc) · 4.56 KB
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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
#include "pointcloudimage.h"
PointCloudImage::PointCloudImage(int imageW,int imageH, bool colorFlag)
{
w = imageW;
h = imageH;
points = cv::Mat(h, w, CV_32FC3);
if(colorFlag == true)
color = cv::Mat(h, w, CV_8UC3,cv::Scalar(0));//由于相机所摄为灰度图像，因此将32FC3改为8UC3
else
color = NULL;
numOfPointsForPixel = cv::Mat(h, w, CV_8U, cv::Scalar(0));
}
PointCloudImage::~PointCloudImage(void)
{
}
bool PointCloudImage::setPoint(int i_w, int j_h, cv::Point3f point, cv::Vec3i colorgray)
{
if(i_w>=w || j_h>=h)
return false;
setPoint(i_w,j_h,point);
Utilities::matSet3D(color,i_w,j_h,colorgray);
return true;
}
bool PointCloudImage::setPoint(int i_w, int j_h, cv::Point3f point)
{
if(i_w>=w || j_h>=h)
return false;
Utilities::matSet3D(points, i_w, j_h, (cv::Vec3f)point);
Utilities::matSet2D(numOfPointsForPixel, j_h, i_w, 1);
return true;
}
bool PointCloudImage::getPoint(int i_w, int j_h, cv::Point3f &pointOut, cv::Vec3i &colorOut)
{
if(i_w>=w || j_h>=h)
return false;
uchar num = numOfPointsForPixel.at<uchar>(j_h,i_w);
if(num > 0){
pointOut = (cv::Point3f) (Utilities::matGet3D(points,i_w,j_h) / (float) num);
if(!color.empty())
colorOut = (cv::Point3i) (Utilities::matGet3D(color,i_w,j_h) / (float) num);
else
colorOut = (cv::Point3i) (100,100,100);//如果color为空，则人为赋一个值
return true;
}
else
return false;
}
bool PointCloudImage::getPoint(int i_w, int j_h, cv::Point3f &pointOut)
{
if(i_w>=w || j_h>=h)
return false;
uchar num = numOfPointsForPixel.at<uchar>(j_h,i_w);
if(num > 0){
pointOut = (cv::Point3f) (Utilities::matGet3D(points, i_w, j_h) / (float) num);
return true;
}
else
return false;
}
bool PointCloudImage::addPoint(int i_w, int j_h, cv::Point3f point, cv::Vec3i colorgray)
{
if(i_w >= w || j_h >= h)//由于i_w为[0,w)取值,因此其不可能等于w，若出现等于，直接判定false
return false;
uchar num = numOfPointsForPixel.at<uchar>(j_h, i_w);
if(num == 0)
return setPoint(i_w, j_h, point, colorgray);
addPoint(i_w, j_h, point);
if(!color.empty()){
cv::Vec3i c = Utilities::matGet3D(color, i_w, j_h);
Utilities::matSet3D(color, i_w, j_h, colorgray + c);
}
else
return false;
return true;
}
bool PointCloudImage::addPoint(int i_w, int j_h, cv::Point3f point)
{
if(i_w>=w || j_h>=h)
return false;
uchar num = numOfPointsForPixel.at<uchar>(j_h,i_w);
if(num == 0)
return setPoint(i_w,j_h,point);
cv::Point3f p = Utilities::matGet3D(points,i_w,j_h);
Utilities::matSet3D(points,i_w,j_h,(cv::Vec3f)(point + p));
numOfPointsForPixel.at<uchar>(j_h,i_w) = num + 1;
return true;
}
void PointCloudImage::exportXYZ(char path[], bool exportOffPixels, bool colorFlag)
{
std::ofstream out;
out.open(path);
cv::Point3f p;
cv::Vec3i c;
for(int i = 0; i<w; i++){
for(int j = 0; j<h; j++){
uchar num = numOfPointsForPixel.at<uchar>(j,i);
if(!exportOffPixels && num == 0)
continue;
getPoint(i,j,p,c);
if(exportOffPixels && num == 0){
p = cv::Point3f(0,0,0);
c = cv::Point3i(0,0,0);
}
out<<p.x<<" "<<p.y<<" "<<p.z;
if(colorFlag && !color.empty())
out<<" "<<c[2]<<" "<<c[1]<<" "<<c[0]<<"\n";
else
out<<"\n";
}
}
out.close();
}
void PointCloudImage::exportNumOfPointsPerPixelImg(char path[])
{
cv::Mat projToCamRays(cvSize(w, h), CV_8U);
float max=0;
int maxX,maxY;
for(int i=0; i<w; i++){
for(int j=0; j<h; j++){
uchar num = numOfPointsForPixel.at<uchar>(j,i);
if(num > max){
max = num;
maxX=i;
maxY=j;
}
}
}
for(int i=0; i<w; i++){
for(int j=0; j<h; j++){
uchar num = numOfPointsForPixel.at<uchar>(j,i);
Utilities::matSet2D(projToCamRays, j, i, num/(float)(max * 255.0));
}
}
cv::imwrite("reconstruction/projToCamRays.png",projToCamRays);
std::ofstream out1;
std::stringstream txt;
txt<<path<<".txt";
out1.open(txt.str().c_str() );
out1<< "black color = 0\nwhite color = "<< max <<"\nmax Pixel: ("<<maxX<<","<<maxY<<")";
out1.close();
}
int PointCloudImage::getWidth()
{
return w;
}
int PointCloudImage::getHeight()
{
return h;
}