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
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
|
diff --git a/src/libmv/simple_pipeline/camera_intrinsics.cc b/src/libmv/simple_pipeline/camera_intrinsics.cc
index f9888ff..110a16d 100644
--- a/src/libmv/simple_pipeline/camera_intrinsics.cc
+++ b/src/libmv/simple_pipeline/camera_intrinsics.cc
@@ -23,7 +23,32 @@
namespace libmv {
-struct Offset { signed char ix,iy; unsigned char fx,fy; };
+struct Offset {
+ signed char ix, iy;
+ unsigned char fx,fy;
+};
+
+struct Grid {
+ struct Offset *offset;
+ int width, height;
+};
+
+static struct Grid *copyGrid(struct Grid *from)
+{
+ struct Grid *to = NULL;
+
+ if (from) {
+ to = new Grid;
+
+ to->width = from->width;
+ to->height = from->height;
+
+ to->offset = new Offset[to->width*to->height];
+ memcpy(to->offset, from->offset, sizeof(struct Offset)*to->width*to->height);
+ }
+
+ return to;
+}
CameraIntrinsics::CameraIntrinsics()
: K_(Mat3::Identity()),
@@ -37,9 +62,22 @@ CameraIntrinsics::CameraIntrinsics()
distort_(0),
undistort_(0) {}
+CameraIntrinsics::CameraIntrinsics(const CameraIntrinsics &from)
+ : K_(from.K_),
+ image_width_(from.image_width_),
+ image_height_(from.image_height_),
+ k1_(from.k1_),
+ k2_(from.k2_),
+ k3_(from.k3_),
+ p1_(from.p1_),
+ p2_(from.p2_)
+{
+ distort_ = copyGrid(from.distort_);
+ undistort_ = copyGrid(from.undistort_);
+}
+
CameraIntrinsics::~CameraIntrinsics() {
- if(distort_) delete[] distort_;
- if(undistort_) delete[] undistort_;
+ FreeLookupGrid();
}
/// Set the entire calibration matrix at once.
@@ -146,11 +184,17 @@ void CameraIntrinsics::InvertIntrinsics(double image_x,
// TODO(MatthiasF): downsample lookup
template<typename WarpFunction>
-void CameraIntrinsics::ComputeLookupGrid(Offset* grid, int width, int height) {
+void CameraIntrinsics::ComputeLookupGrid(Grid* grid, int width, int height) {
+ double aspx = (double)width / image_width_;
+ double aspy = (double)height / image_height_;
+
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
+ double src_x = x / aspx, src_y = y / aspy;
double warp_x, warp_y;
- WarpFunction(this,x,y,&warp_x,&warp_y);
+ WarpFunction(this,src_x,src_y,&warp_x,&warp_y);
+ warp_x = warp_x*aspx;
+ warp_y = warp_y*aspy;
int ix = int(warp_x), iy = int(warp_y);
int fx = round((warp_x-ix)*256), fy = round((warp_y-iy)*256);
if(fx == 256) { fx=0; ix++; }
@@ -162,10 +206,10 @@ void CameraIntrinsics::ComputeLookupGrid(Offset* grid, int width, int height) {
if( iy >= height-2 ) iy = height-2;
if ( ix-x > -128 && ix-x < 128 && iy-y > -128 && iy-y < 128 ) {
Offset offset = { ix-x, iy-y, fx, fy };
- grid[y*width+x] = offset;
+ grid->offset[y*width+x] = offset;
} else {
Offset offset = { 0, 0, 0, 0 };
- grid[y*width+x] = offset;
+ grid->offset[y*width+x] = offset;
}
}
}
@@ -173,11 +217,11 @@ void CameraIntrinsics::ComputeLookupGrid(Offset* grid, int width, int height) {
// TODO(MatthiasF): cubic B-Spline image sampling, bilinear lookup
template<typename T,int N>
-static void Warp(const Offset* grid, const T* src, T* dst,
+static void Warp(const Grid* grid, const T* src, T* dst,
int width, int height) {
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
- Offset offset = grid[y*width+x];
+ Offset offset = grid->offset[y*width+x];
const T* s = &src[((y+offset.iy)*width+(x+offset.ix))*N];
for (int i = 0; i < N; i++) {
dst[(y*width+x)*N+i] = ((s[ i] * (256-offset.fx) + s[ N+i] * offset.fx) * (256-offset.fy)
@@ -188,8 +232,17 @@ static void Warp(const Offset* grid, const T* src, T* dst,
}
void CameraIntrinsics::FreeLookupGrid() {
- if(distort_) delete distort_, distort_=0;
- if(undistort_) delete undistort_, undistort_=0;
+ if(distort_) {
+ delete distort_->offset;
+ delete distort_;
+ distort_ = NULL;
+ }
+
+ if(undistort_) {
+ delete undistort_->offset;
+ delete undistort_;
+ undistort_ = NULL;
+ }
}
// FIXME: C++ templates limitations makes thing complicated, but maybe there is a simpler method.
@@ -211,11 +264,50 @@ struct InvertIntrinsicsFunction {
}
};
-void CameraIntrinsics::Distort(const float* src, float* dst, int width, int height, int channels) {
- if(!distort_) {
- distort_ = new Offset[width*height];
- ComputeLookupGrid<InvertIntrinsicsFunction>(distort_,width,height);
+void CameraIntrinsics::CheckDistortLookupGrid(int width, int height)
+{
+ if(distort_) {
+ if(distort_->width != width || distort_->height != height) {
+ delete [] distort_->offset;
+ distort_->offset = NULL;
+ }
+ } else {
+ distort_ = new Grid;
+ distort_->offset = NULL;
+ }
+
+ if(!distort_->offset) {
+ distort_->offset = new Offset[width*height];
+ ComputeLookupGrid<InvertIntrinsicsFunction>(distort_,width,height);
}
+
+ distort_->width = width;
+ distort_->height = height;
+}
+
+void CameraIntrinsics::CheckUndistortLookupGrid(int width, int height)
+{
+ if(undistort_) {
+ if(undistort_->width != width || undistort_->height != height) {
+ delete [] undistort_->offset;
+ undistort_->offset = NULL;
+ }
+ } else {
+ undistort_ = new Grid;
+ undistort_->offset = NULL;
+ }
+
+ if(!undistort_->offset) {
+ undistort_->offset = new Offset[width*height];
+ ComputeLookupGrid<ApplyIntrinsicsFunction>(undistort_,width,height);
+ }
+
+ undistort_->width = width;
+ undistort_->height = height;
+}
+
+void CameraIntrinsics::Distort(const float* src, float* dst, int width, int height, int channels) {
+ CheckDistortLookupGrid(width, height);
if(channels==1) Warp<float,1>(distort_,src,dst,width,height);
else if(channels==2) Warp<float,2>(distort_,src,dst,width,height);
else if(channels==3) Warp<float,3>(distort_,src,dst,width,height);
@@ -224,10 +316,7 @@ void CameraIntrinsics::Distort(const float* src, float* dst, int width, int heig
}
void CameraIntrinsics::Distort(const unsigned char* src, unsigned char* dst, int width, int height, int channels) {
- if(!distort_) {
- distort_ = new Offset[width*height];
- ComputeLookupGrid<InvertIntrinsicsFunction>(distort_,width,height);
- }
+ CheckDistortLookupGrid(width, height);
if(channels==1) Warp<unsigned char,1>(distort_,src,dst,width,height);
else if(channels==2) Warp<unsigned char,2>(distort_,src,dst,width,height);
else if(channels==3) Warp<unsigned char,3>(distort_,src,dst,width,height);
@@ -236,10 +325,7 @@ void CameraIntrinsics::Distort(const unsigned char* src, unsigned char* dst, int
}
void CameraIntrinsics::Undistort(const float* src, float* dst, int width, int height, int channels) {
- if(!undistort_) {
- undistort_ = new Offset[width*height];
- ComputeLookupGrid<ApplyIntrinsicsFunction>(undistort_,width,height);
- }
+ CheckUndistortLookupGrid(width, height);
if(channels==1) Warp<float,1>(undistort_,src,dst,width,height);
else if(channels==2) Warp<float,2>(undistort_,src,dst,width,height);
else if(channels==3) Warp<float,3>(undistort_,src,dst,width,height);
@@ -248,10 +334,7 @@ void CameraIntrinsics::Undistort(const float* src, float* dst, int width, int he
}
void CameraIntrinsics::Undistort(const unsigned char* src, unsigned char* dst, int width, int height, int channels) {
- if(!undistort_) {
- undistort_ = new Offset[width*height];
- ComputeLookupGrid<ApplyIntrinsicsFunction>(undistort_,width,height);
- }
+ CheckUndistortLookupGrid(width, height);
if(channels==1) Warp<unsigned char,1>(undistort_,src,dst,width,height);
else if(channels==2) Warp<unsigned char,2>(undistort_,src,dst,width,height);
else if(channels==3) Warp<unsigned char,3>(undistort_,src,dst,width,height);
diff --git a/src/libmv/simple_pipeline/camera_intrinsics.h b/src/libmv/simple_pipeline/camera_intrinsics.h
index 29bc8a1..f525571 100644
--- a/src/libmv/simple_pipeline/camera_intrinsics.h
+++ b/src/libmv/simple_pipeline/camera_intrinsics.h
@@ -26,11 +26,12 @@ typedef Eigen::Matrix<double, 3, 3> Mat3;
namespace libmv {
-struct Offset;
+struct Grid;
class CameraIntrinsics {
public:
CameraIntrinsics();
+ CameraIntrinsics(const CameraIntrinsics &from);
~CameraIntrinsics();
const Mat3 &K() const { return K_; }
@@ -123,7 +124,9 @@ class CameraIntrinsics {
int width, int height, int channels);
private:
- template<typename WarpFunction> void ComputeLookupGrid(Offset* grid, int width, int height);
+ template<typename WarpFunction> void ComputeLookupGrid(struct Grid* grid, int width, int height);
+ void CheckUndistortLookupGrid(int width, int height);
+ void CheckDistortLookupGrid(int width, int height);
void FreeLookupGrid();
// The traditional intrinsics matrix from x = K[R|t]X.
@@ -140,8 +143,8 @@ class CameraIntrinsics {
// independent of image size.
double k1_, k2_, k3_, p1_, p2_;
- Offset* distort_;
- Offset* undistort_;
+ struct Grid *distort_;
+ struct Grid *undistort_;
};
} // namespace libmv
|
