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 #include "base/camera_rig.h"
33
34 #include "util/misc.h"
35
36 namespace colmap {
37
CameraRig()38 CameraRig::CameraRig() {}
39
NumCameras() const40 size_t CameraRig::NumCameras() const { return rig_cameras_.size(); }
41
NumSnapshots() const42 size_t CameraRig::NumSnapshots() const { return snapshots_.size(); }
43
HasCamera(const camera_t camera_id) const44 bool CameraRig::HasCamera(const camera_t camera_id) const {
45 return rig_cameras_.count(camera_id);
46 }
47
RefCameraId() const48 camera_t CameraRig::RefCameraId() const { return ref_camera_id_; }
49
SetRefCameraId(const camera_t camera_id)50 void CameraRig::SetRefCameraId(const camera_t camera_id) {
51 CHECK(HasCamera(camera_id));
52 ref_camera_id_ = camera_id;
53 }
54
GetCameraIds() const55 std::vector<camera_t> CameraRig::GetCameraIds() const {
56 std::vector<camera_t> rig_camera_ids;
57 rig_camera_ids.reserve(NumCameras());
58 for (const auto& rig_camera : rig_cameras_) {
59 rig_camera_ids.push_back(rig_camera.first);
60 }
61 return rig_camera_ids;
62 }
63
Snapshots() const64 const std::vector<std::vector<image_t>>& CameraRig::Snapshots() const {
65 return snapshots_;
66 }
67
AddCamera(const camera_t camera_id,const Eigen::Vector4d & rel_qvec,const Eigen::Vector3d & rel_tvec)68 void CameraRig::AddCamera(const camera_t camera_id,
69 const Eigen::Vector4d& rel_qvec,
70 const Eigen::Vector3d& rel_tvec) {
71 CHECK(!HasCamera(camera_id));
72 CHECK_EQ(NumSnapshots(), 0);
73 RigCamera rig_camera;
74 rig_camera.rel_qvec = rel_qvec;
75 rig_camera.rel_tvec = rel_tvec;
76 rig_cameras_.emplace(camera_id, rig_camera);
77 }
78
AddSnapshot(const std::vector<image_t> & image_ids)79 void CameraRig::AddSnapshot(const std::vector<image_t>& image_ids) {
80 CHECK(!image_ids.empty());
81 CHECK_LE(image_ids.size(), NumCameras());
82 CHECK(!VectorContainsDuplicateValues(image_ids));
83 snapshots_.push_back(image_ids);
84 }
85
Check(const Reconstruction & reconstruction) const86 void CameraRig::Check(const Reconstruction& reconstruction) const {
87 CHECK(HasCamera(ref_camera_id_));
88
89 for (const auto& rig_camera : rig_cameras_) {
90 CHECK(reconstruction.ExistsCamera(rig_camera.first));
91 }
92
93 std::unordered_set<image_t> all_image_ids;
94 for (const auto& snapshot : snapshots_) {
95 CHECK(!snapshot.empty());
96 CHECK_LE(snapshot.size(), NumCameras());
97 bool has_ref_camera = false;
98 for (const auto image_id : snapshot) {
99 CHECK(reconstruction.ExistsImage(image_id));
100 CHECK_EQ(all_image_ids.count(image_id), 0);
101 all_image_ids.insert(image_id);
102 const auto& image = reconstruction.Image(image_id);
103 CHECK(HasCamera(image.CameraId()));
104 if (image.CameraId() == ref_camera_id_) {
105 has_ref_camera = true;
106 }
107 }
108 CHECK(has_ref_camera);
109 }
110 }
111
RelativeQvec(const camera_t camera_id)112 Eigen::Vector4d& CameraRig::RelativeQvec(const camera_t camera_id) {
113 return rig_cameras_.at(camera_id).rel_qvec;
114 }
115
RelativeQvec(const camera_t camera_id) const116 const Eigen::Vector4d& CameraRig::RelativeQvec(const camera_t camera_id) const {
117 return rig_cameras_.at(camera_id).rel_qvec;
118 }
119
RelativeTvec(const camera_t camera_id)120 Eigen::Vector3d& CameraRig::RelativeTvec(const camera_t camera_id) {
121 return rig_cameras_.at(camera_id).rel_tvec;
122 }
123
RelativeTvec(const camera_t camera_id) const124 const Eigen::Vector3d& CameraRig::RelativeTvec(const camera_t camera_id) const {
125 return rig_cameras_.at(camera_id).rel_tvec;
126 }
127
ComputeScale(const Reconstruction & reconstruction) const128 double CameraRig::ComputeScale(const Reconstruction& reconstruction) const {
129 CHECK_GT(NumSnapshots(), 0);
130 CHECK_GT(NumCameras(), 0);
131 double scaling_factor = 0;
132 size_t num_dists = 0;
133 std::vector<Eigen::Vector3d> rel_proj_centers(NumCameras());
134 std::vector<Eigen::Vector3d> abs_proj_centers(NumCameras());
135 for (const auto& snapshot : snapshots_) {
136 // Compute the projection relative and absolute projection centers.
137 for (size_t i = 0; i < NumCameras(); ++i) {
138 const auto& image = reconstruction.Image(snapshot[i]);
139 rel_proj_centers[i] = ProjectionCenterFromPose(
140 RelativeQvec(image.CameraId()), RelativeTvec(image.CameraId()));
141 abs_proj_centers[i] = image.ProjectionCenter();
142 }
143
144 // Accumulate the scaling factor for all pairs of camera distances.
145 for (size_t i = 0; i < NumCameras(); ++i) {
146 for (size_t j = 0; j < i; ++j) {
147 const double rel_dist =
148 (rel_proj_centers[i] - rel_proj_centers[j]).norm();
149 const double abs_dist =
150 (abs_proj_centers[i] - abs_proj_centers[j]).norm();
151 const double kMinDist = 1e-6;
152 if (rel_dist > kMinDist && abs_dist > kMinDist) {
153 scaling_factor += rel_dist / abs_dist;
154 num_dists += 1;
155 }
156 }
157 }
158 }
159
160 if (num_dists == 0) {
161 return std::numeric_limits<double>::quiet_NaN();
162 }
163
164 return scaling_factor / num_dists;
165 }
166
ComputeRelativePoses(const Reconstruction & reconstruction)167 void CameraRig::ComputeRelativePoses(const Reconstruction& reconstruction) {
168 CHECK_GT(NumSnapshots(), 0);
169 CHECK_NE(ref_camera_id_, kInvalidCameraId);
170
171 for (auto& rig_camera : rig_cameras_) {
172 rig_camera.second.rel_tvec = Eigen::Vector3d::Zero();
173 }
174
175 EIGEN_STL_UMAP(camera_t, std::vector<Eigen::Vector4d>) rel_qvecs;
176
177 for (const auto& snapshot : snapshots_) {
178 // Find the image of the reference camera in the current snapshot.
179 const Image* ref_image = nullptr;
180 for (const auto image_id : snapshot) {
181 const auto& image = reconstruction.Image(image_id);
182 if (image.CameraId() == ref_camera_id_) {
183 ref_image = ℑ
184 }
185 }
186
187 CHECK_NOTNULL(ref_image);
188
189 // Compute the relative poses from all cameras in the current snapshot to
190 // the reference camera.
191 for (const auto image_id : snapshot) {
192 const auto& image = reconstruction.Image(image_id);
193 if (image.CameraId() != ref_camera_id_) {
194 Eigen::Vector4d rel_qvec;
195 Eigen::Vector3d rel_tvec;
196 ComputeRelativePose(ref_image->Qvec(), ref_image->Tvec(), image.Qvec(),
197 image.Tvec(), &rel_qvec, &rel_tvec);
198
199 rel_qvecs[image.CameraId()].push_back(rel_qvec);
200 RelativeTvec(image.CameraId()) += rel_tvec;
201 }
202 }
203 }
204
205 RelativeQvec(ref_camera_id_) = ComposeIdentityQuaternion();
206 RelativeTvec(ref_camera_id_) = Eigen::Vector3d(0, 0, 0);
207
208 // Compute the average relative poses.
209 for (auto& rig_camera : rig_cameras_) {
210 if (rig_camera.first != ref_camera_id_) {
211 CHECK_GT(rel_qvecs.count(rig_camera.first), 0)
212 << "Need at least one snapshot with an image of camera "
213 << rig_camera.first << " and the reference camera " << ref_camera_id_
214 << " to compute its relative pose in the camera rig";
215 const std::vector<Eigen::Vector4d>& camera_rel_qvecs =
216 rel_qvecs.at(rig_camera.first);
217 const std::vector<double> rel_qvec_weights(camera_rel_qvecs.size(), 1.0);
218 rig_camera.second.rel_qvec =
219 AverageQuaternions(camera_rel_qvecs, rel_qvec_weights);
220 rig_camera.second.rel_tvec /= camera_rel_qvecs.size();
221 }
222 }
223 }
224
ComputeAbsolutePose(const size_t snapshot_idx,const Reconstruction & reconstruction,Eigen::Vector4d * abs_qvec,Eigen::Vector3d * abs_tvec) const225 void CameraRig::ComputeAbsolutePose(const size_t snapshot_idx,
226 const Reconstruction& reconstruction,
227 Eigen::Vector4d* abs_qvec,
228 Eigen::Vector3d* abs_tvec) const {
229 const auto& snapshot = snapshots_.at(snapshot_idx);
230
231 std::vector<Eigen::Vector4d> abs_qvecs;
232 *abs_tvec = Eigen::Vector3d::Zero();
233
234 for (const auto image_id : snapshot) {
235 const auto& image = reconstruction.Image(image_id);
236 Eigen::Vector4d inv_rel_qvec;
237 Eigen::Vector3d inv_rel_tvec;
238 InvertPose(RelativeQvec(image.CameraId()), RelativeTvec(image.CameraId()),
239 &inv_rel_qvec, &inv_rel_tvec);
240
241 const Eigen::Vector4d qvec =
242 ConcatenateQuaternions(image.Qvec(), inv_rel_qvec);
243 const Eigen::Vector3d tvec = QuaternionRotatePoint(
244 inv_rel_qvec, image.Tvec() - RelativeTvec(image.CameraId()));
245
246 abs_qvecs.push_back(qvec);
247 *abs_tvec += tvec;
248 }
249
250 const std::vector<double> abs_qvec_weights(snapshot.size(), 1);
251 *abs_qvec = AverageQuaternions(abs_qvecs, abs_qvec_weights);
252 *abs_tvec /= snapshot.size();
253 }
254
255 } // namespace colmap
256