ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
camera_rig_test.cc
Go to the documentation of this file.
1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 //
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 //
14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 // its contributors may be used to endorse or promote products derived
16 // from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31 
32 #define TEST_NAME "base/camera_rig"
33 #include "util/testing.h"
34 
35 #include "base/camera_rig.h"
36 
37 using namespace colmap;
38 
39 BOOST_AUTO_TEST_CASE(TestEmpty) {
40  CameraRig camera_rig;
41  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 0);
42  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
43  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds().size(), 0);
44  BOOST_CHECK_EQUAL(camera_rig.HasCamera(0), false);
45 }
46 
47 BOOST_AUTO_TEST_CASE(TestAddCamera) {
48  CameraRig camera_rig;
49  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 0);
50  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
51  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds().size(), 0);
52  BOOST_CHECK_EQUAL(camera_rig.HasCamera(0), false);
53 
54  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
55  Eigen::Vector3d(0, 1, 2));
56  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 1);
57  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
58  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds().size(), 1);
59  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds()[0], 0);
60  BOOST_CHECK_EQUAL(camera_rig.HasCamera(0), true);
61  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(0), ComposeIdentityQuaternion());
62  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(0), Eigen::Vector3d(0, 1, 2));
63 
64  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
65  Eigen::Vector3d(3, 4, 5));
66  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 2);
67  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
68  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds().size(), 2);
69  BOOST_CHECK_EQUAL(camera_rig.HasCamera(0), true);
70  BOOST_CHECK_EQUAL(camera_rig.HasCamera(1), true);
71  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(1), ComposeIdentityQuaternion());
72  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(1), Eigen::Vector3d(3, 4, 5));
73 }
74 
75 BOOST_AUTO_TEST_CASE(TestAddSnapshot) {
76  CameraRig camera_rig;
77  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 0);
78  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
79  BOOST_CHECK_EQUAL(camera_rig.GetCameraIds().size(), 0);
80  BOOST_CHECK_EQUAL(camera_rig.Snapshots().size(), 0);
81 
82  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
83  Eigen::Vector3d(0, 1, 2));
84  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
85  Eigen::Vector3d(3, 4, 5));
86  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 2);
87  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 0);
88  BOOST_CHECK_EQUAL(camera_rig.Snapshots().size(), 0);
89 
90  const std::vector<image_t> image_ids1 = {0, 1};
91  camera_rig.AddSnapshot(image_ids1);
92  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 2);
93  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 1);
94  BOOST_CHECK_EQUAL(camera_rig.Snapshots().size(), 1);
95  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0].size(), 2);
96  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0][0], 0);
97  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0][1], 1);
98 
99  const std::vector<image_t> image_ids2 = {2, 3};
100  camera_rig.AddSnapshot(image_ids2);
101  BOOST_CHECK_EQUAL(camera_rig.NumCameras(), 2);
102  BOOST_CHECK_EQUAL(camera_rig.NumSnapshots(), 2);
103  BOOST_CHECK_EQUAL(camera_rig.Snapshots().size(), 2);
104  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0].size(), 2);
105  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0][0], 0);
106  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[0][1], 1);
107  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[1].size(), 2);
108  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[1][0], 2);
109  BOOST_CHECK_EQUAL(camera_rig.Snapshots()[1][1], 3);
110 }
111 
112 BOOST_AUTO_TEST_CASE(TestCheck) {
113  CameraRig camera_rig;
114  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
115  Eigen::Vector3d(0, 1, 2));
116  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
117  Eigen::Vector3d(3, 4, 5));
118  const std::vector<image_t> image_ids1 = {0, 1};
119  camera_rig.AddSnapshot(image_ids1);
120  const std::vector<image_t> image_ids2 = {2, 3};
121  camera_rig.AddSnapshot(image_ids2);
122 
123  Reconstruction reconstruction;
124 
125  Camera camera1;
126  camera1.SetCameraId(0);
127  camera1.InitializeWithName("PINHOLE", 1, 1, 1);
128  reconstruction.AddCamera(camera1);
129 
130  Camera camera2;
131  camera2.SetCameraId(1);
132  camera2.InitializeWithName("PINHOLE", 1, 1, 1);
133  reconstruction.AddCamera(camera2);
134 
135  Image image1;
136  image1.SetImageId(0);
137  image1.SetCameraId(camera1.CameraId());
138  reconstruction.AddImage(image1);
139 
140  Image image2;
141  image2.SetImageId(1);
142  image2.SetCameraId(camera2.CameraId());
143  reconstruction.AddImage(image2);
144 
145  Image image3;
146  image3.SetImageId(2);
147  image3.SetCameraId(camera1.CameraId());
148  reconstruction.AddImage(image3);
149 
150  Image image4;
151  image4.SetImageId(3);
152  image4.SetCameraId(camera2.CameraId());
153  reconstruction.AddImage(image4);
154 
155  camera_rig.SetRefCameraId(0);
156  camera_rig.Check(reconstruction);
157 }
158 
159 BOOST_AUTO_TEST_CASE(TestComputeScale) {
160  CameraRig camera_rig;
161  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
162  Eigen::Vector3d(0, 0, 0));
163  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
164  Eigen::Vector3d(2, 4, 6));
165  const std::vector<image_t> image_ids1 = {0, 1};
166  camera_rig.AddSnapshot(image_ids1);
167 
168  Reconstruction reconstruction;
169 
170  Camera camera1;
171  camera1.SetCameraId(0);
172  camera1.InitializeWithName("PINHOLE", 1, 1, 1);
173  reconstruction.AddCamera(camera1);
174 
175  Camera camera2;
176  camera2.SetCameraId(1);
177  camera2.InitializeWithName("PINHOLE", 1, 1, 1);
178  reconstruction.AddCamera(camera2);
179 
180  Image image1;
181  image1.SetImageId(0);
182  image1.SetCameraId(camera1.CameraId());
183  image1.SetQvec(ComposeIdentityQuaternion());
184  image1.SetTvec(Eigen::Vector3d(0, 0, 0));
185  reconstruction.AddImage(image1);
186 
187  Image image2;
188  image2.SetImageId(1);
189  image2.SetCameraId(camera2.CameraId());
190  image2.SetQvec(ComposeIdentityQuaternion());
191  image2.SetTvec(Eigen::Vector3d(1, 2, 3));
192  reconstruction.AddImage(image2);
193 
194  camera_rig.SetRefCameraId(0);
195  camera_rig.Check(reconstruction);
196 
197  BOOST_CHECK_EQUAL(camera_rig.ComputeScale(reconstruction), 2.0);
198 
199  reconstruction.Image(1).SetTvec(Eigen::Vector3d(0, 0, 0));
200  BOOST_CHECK(IsNaN(camera_rig.ComputeScale(reconstruction)));
201 }
202 
203 BOOST_AUTO_TEST_CASE(TestComputeRelativePoses) {
204  CameraRig camera_rig;
205  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
206  Eigen::Vector3d(0, 0, 0));
207  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
208  Eigen::Vector3d(0, 0, 0));
209  const std::vector<image_t> image_ids1 = {0, 1};
210  camera_rig.AddSnapshot(image_ids1);
211 
212  Reconstruction reconstruction;
213 
214  Camera camera1;
215  camera1.SetCameraId(0);
216  camera1.InitializeWithName("PINHOLE", 1, 1, 1);
217  reconstruction.AddCamera(camera1);
218 
219  Camera camera2;
220  camera2.SetCameraId(1);
221  camera2.InitializeWithName("PINHOLE", 1, 1, 1);
222  reconstruction.AddCamera(camera2);
223 
224  Image image1;
225  image1.SetImageId(0);
226  image1.SetCameraId(camera1.CameraId());
227  image1.SetQvec(ComposeIdentityQuaternion());
228  image1.SetTvec(Eigen::Vector3d(0, 0, 0));
229  reconstruction.AddImage(image1);
230 
231  Image image2;
232  image2.SetImageId(1);
233  image2.SetCameraId(camera2.CameraId());
234  image2.SetQvec(ComposeIdentityQuaternion());
235  image2.SetTvec(Eigen::Vector3d(1, 2, 3));
236  reconstruction.AddImage(image2);
237 
238  camera_rig.SetRefCameraId(0);
239  camera_rig.Check(reconstruction);
240  camera_rig.ComputeRelativePoses(reconstruction);
241  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(0), ComposeIdentityQuaternion());
242  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(0), Eigen::Vector3d(0, 0, 0));
243  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(1), ComposeIdentityQuaternion());
244  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(1), Eigen::Vector3d(1, 2, 3));
245 
246  const std::vector<image_t> image_ids2 = {2, 3};
247  camera_rig.AddSnapshot(image_ids2);
248 
249  Image image3;
250  image3.SetImageId(2);
251  image3.SetCameraId(camera1.CameraId());
252  image3.SetQvec(ComposeIdentityQuaternion());
253  image3.SetTvec(Eigen::Vector3d(0, 0, 0));
254  reconstruction.AddImage(image3);
255 
256  Image image4;
257  image4.SetImageId(3);
258  image4.SetCameraId(camera2.CameraId());
259  image4.SetQvec(ComposeIdentityQuaternion());
260  image4.SetTvec(Eigen::Vector3d(2, 4, 6));
261  reconstruction.AddImage(image4);
262 
263  camera_rig.Check(reconstruction);
264  camera_rig.ComputeRelativePoses(reconstruction);
265  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(0), ComposeIdentityQuaternion());
266  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(0), Eigen::Vector3d(0, 0, 0));
267  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(1), ComposeIdentityQuaternion());
268  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(1), Eigen::Vector3d(1.5, 3, 4.5));
269 
270  const std::vector<image_t> image_ids3 = {4};
271  camera_rig.AddSnapshot(image_ids3);
272 
273  Image image5;
274  image5.SetImageId(4);
275  image5.SetCameraId(camera1.CameraId());
276  image5.SetQvec(ComposeIdentityQuaternion());
277  image5.SetTvec(Eigen::Vector3d(0, 0, 0));
278  reconstruction.AddImage(image5);
279 
280  camera_rig.Check(reconstruction);
281  camera_rig.ComputeRelativePoses(reconstruction);
282  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(0), ComposeIdentityQuaternion());
283  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(0), Eigen::Vector3d(0, 0, 0));
284  BOOST_CHECK_EQUAL(camera_rig.RelativeQvec(1), ComposeIdentityQuaternion());
285  BOOST_CHECK_EQUAL(camera_rig.RelativeTvec(1), Eigen::Vector3d(1.5, 3, 4.5));
286 }
287 
288 BOOST_AUTO_TEST_CASE(TestComputeAbsolutePose) {
289  CameraRig camera_rig;
290  camera_rig.AddCamera(0, ComposeIdentityQuaternion(),
291  Eigen::Vector3d(0, 1, 2));
292  camera_rig.AddCamera(1, ComposeIdentityQuaternion(),
293  Eigen::Vector3d(3, 4, 5));
294  const std::vector<image_t> image_ids1 = {0, 1};
295  camera_rig.AddSnapshot(image_ids1);
296 
297  Reconstruction reconstruction;
298 
299  Camera camera1;
300  camera1.SetCameraId(0);
301  camera1.InitializeWithName("PINHOLE", 1, 1, 1);
302  reconstruction.AddCamera(camera1);
303 
304  Camera camera2;
305  camera2.SetCameraId(1);
306  camera2.InitializeWithName("PINHOLE", 1, 1, 1);
307  reconstruction.AddCamera(camera2);
308 
309  Image image1;
310  image1.SetImageId(0);
311  image1.SetCameraId(camera1.CameraId());
312  image1.SetQvec(ComposeIdentityQuaternion());
313  image1.SetTvec(Eigen::Vector3d(0, 0, 0));
314  reconstruction.AddImage(image1);
315 
316  Image image2;
317  image2.SetImageId(1);
318  image2.SetCameraId(camera2.CameraId());
319  image2.SetQvec(ComposeIdentityQuaternion());
320  image2.SetTvec(Eigen::Vector3d(3, 3, 3));
321  reconstruction.AddImage(image2);
322 
323  camera_rig.SetRefCameraId(0);
324  camera_rig.Check(reconstruction);
325 
326  Eigen::Vector4d abs_qvec;
327  Eigen::Vector3d abs_tvec;
328  camera_rig.ComputeAbsolutePose(0, reconstruction, &abs_qvec, &abs_tvec);
329  BOOST_CHECK_EQUAL(abs_qvec, ComposeIdentityQuaternion());
330  BOOST_CHECK_EQUAL(abs_tvec, Eigen::Vector3d(0, -1, -2));
331 }
camera_rig.h
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(TestEmpty)
Definition: camera_rig_test.cc:39
colmap::CameraRig::SetRefCameraId
void SetRefCameraId(const camera_t camera_id)
Definition: camera_rig.cc:50
colmap::CameraRig::Check
void Check(const Reconstruction &reconstruction) const
Definition: camera_rig.cc:86
colmap::CameraRig::AddCamera
void AddCamera(const camera_t camera_id, const Eigen::Vector4d &rel_qvec, const Eigen::Vector3d &rel_tvec)
Definition: camera_rig.cc:68
colmap::CameraRig::HasCamera
bool HasCamera(const camera_t camera_id) const
Definition: camera_rig.cc:44
colmap::CameraRig::NumSnapshots
size_t NumSnapshots() const
Definition: camera_rig.cc:42
colmap::CameraRig::RelativeTvec
Eigen::Vector3d & RelativeTvec(const camera_t camera_id)
Definition: camera_rig.cc:120
colmap::CameraRig::AddSnapshot
void AddSnapshot(const std::vector< image_t > &image_ids)
Definition: camera_rig.cc:79
colmap::CameraRig::NumCameras
size_t NumCameras() const
Definition: camera_rig.cc:40
colmap::CameraRig::ComputeRelativePoses
bool ComputeRelativePoses(const Reconstruction &reconstruction)
Definition: camera_rig.cc:167
colmap::CameraRig::GetCameraIds
std::vector< camera_t > GetCameraIds() const
Definition: camera_rig.cc:55
colmap::CameraRig::Snapshots
const std::vector< std::vector< image_t > > & Snapshots() const
Definition: camera_rig.cc:64
colmap::CameraRig::RelativeQvec
Eigen::Vector4d & RelativeQvec(const camera_t camera_id)
Definition: camera_rig.cc:112
colmap::CameraRig::ComputeAbsolutePose
void ComputeAbsolutePose(const size_t snapshot_idx, const Reconstruction &reconstruction, Eigen::Vector4d *abs_qvec, Eigen::Vector3d *abs_tvec) const
Definition: camera_rig.cc:230
colmap::CameraRig::ComputeScale
double ComputeScale(const Reconstruction &reconstruction) const
Definition: camera_rig.cc:128
colmap::Camera::InitializeWithName
void InitializeWithName(const std::string &model_name, const double focal_length, const size_t width, const size_t height)
Definition: camera.cc:212
colmap::Camera::CameraId
camera_t CameraId() const
Definition: camera.h:154
colmap::Camera::SetCameraId
void SetCameraId(const camera_t camera_id)
Definition: camera.h:156
colmap::Image::SetImageId
void SetImageId(const image_t image_id)
Definition: image.h:240
colmap::Image::SetCameraId
void SetCameraId(const camera_t camera_id)
Definition: image.h:250
colmap::Image::SetQvec
void SetQvec(const Eigen::Vector4d &qvec)
Definition: image.h:309
colmap::Image::SetTvec
void SetTvec(const Eigen::Vector3d &tvec)
Definition: image.h:333
colmap::Reconstruction::AddImage
void AddImage(const class Image &image)
Definition: reconstruction.cc:184
colmap::Reconstruction::AddCamera
void AddCamera(const class Camera &camera)
Definition: reconstruction.cc:178
colmap::Reconstruction::Image
const class Image & Image(const image_t image_id) const
Definition: reconstruction.h:454
colmap::ComposeIdentityQuaternion
Eigen::Vector4d ComposeIdentityQuaternion()
Definition: pose.h:209
colmap::IsNaN
bool IsNaN(const float x)
Definition: math.h:160