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/image.h"
33
34 #include "base/pose.h"
35 #include "base/projection.h"
36
37 namespace colmap {
38 namespace {
39
40 static const double kNaN = std::numeric_limits<double>::quiet_NaN();
41
42 } // namespace
43
44 const int Image::kNumPoint3DVisibilityPyramidLevels = 6;
45
Image()46 Image::Image()
47 : image_id_(kInvalidImageId),
48 name_(""),
49 camera_id_(kInvalidCameraId),
50 registered_(false),
51 num_points3D_(0),
52 num_observations_(0),
53 num_correspondences_(0),
54 num_visible_points3D_(0),
55 qvec_(1.0, 0.0, 0.0, 0.0),
56 tvec_(0.0, 0.0, 0.0),
57 qvec_prior_(kNaN, kNaN, kNaN, kNaN),
58 tvec_prior_(kNaN, kNaN, kNaN) {}
59
SetUp(const class Camera & camera)60 void Image::SetUp(const class Camera& camera) {
61 CHECK_EQ(camera_id_, camera.CameraId());
62 point3D_visibility_pyramid_ = VisibilityPyramid(
63 kNumPoint3DVisibilityPyramidLevels, camera.Width(), camera.Height());
64 }
65
TearDown()66 void Image::TearDown() {
67 point3D_visibility_pyramid_ = VisibilityPyramid(0, 0, 0);
68 }
69
SetPoints2D(const std::vector<Eigen::Vector2d> & points)70 void Image::SetPoints2D(const std::vector<Eigen::Vector2d>& points) {
71 CHECK(points2D_.empty());
72 points2D_.resize(points.size());
73 num_correspondences_have_point3D_.resize(points.size(), 0);
74 for (point2D_t point2D_idx = 0; point2D_idx < points.size(); ++point2D_idx) {
75 points2D_[point2D_idx].SetXY(points[point2D_idx]);
76 }
77 }
78
SetPoints2D(const std::vector<class Point2D> & points)79 void Image::SetPoints2D(const std::vector<class Point2D>& points) {
80 CHECK(points2D_.empty());
81 points2D_ = points;
82 num_correspondences_have_point3D_.resize(points.size(), 0);
83 }
84
SetPoint3DForPoint2D(const point2D_t point2D_idx,const point3D_t point3D_id)85 void Image::SetPoint3DForPoint2D(const point2D_t point2D_idx,
86 const point3D_t point3D_id) {
87 CHECK_NE(point3D_id, kInvalidPoint3DId);
88 class Point2D& point2D = points2D_.at(point2D_idx);
89 if (!point2D.HasPoint3D()) {
90 num_points3D_ += 1;
91 }
92 point2D.SetPoint3DId(point3D_id);
93 }
94
ResetPoint3DForPoint2D(const point2D_t point2D_idx)95 void Image::ResetPoint3DForPoint2D(const point2D_t point2D_idx) {
96 class Point2D& point2D = points2D_.at(point2D_idx);
97 if (point2D.HasPoint3D()) {
98 point2D.SetPoint3DId(kInvalidPoint3DId);
99 num_points3D_ -= 1;
100 }
101 }
102
HasPoint3D(const point3D_t point3D_id) const103 bool Image::HasPoint3D(const point3D_t point3D_id) const {
104 return std::find_if(points2D_.begin(), points2D_.end(),
105 [point3D_id](const class Point2D& point2D) {
106 return point2D.Point3DId() == point3D_id;
107 }) != points2D_.end();
108 }
109
IncrementCorrespondenceHasPoint3D(const point2D_t point2D_idx)110 void Image::IncrementCorrespondenceHasPoint3D(const point2D_t point2D_idx) {
111 const class Point2D& point2D = points2D_.at(point2D_idx);
112
113 num_correspondences_have_point3D_[point2D_idx] += 1;
114 if (num_correspondences_have_point3D_[point2D_idx] == 1) {
115 num_visible_points3D_ += 1;
116 }
117
118 point3D_visibility_pyramid_.SetPoint(point2D.X(), point2D.Y());
119
120 assert(num_visible_points3D_ <= num_observations_);
121 }
122
DecrementCorrespondenceHasPoint3D(const point2D_t point2D_idx)123 void Image::DecrementCorrespondenceHasPoint3D(const point2D_t point2D_idx) {
124 const class Point2D& point2D = points2D_.at(point2D_idx);
125
126 num_correspondences_have_point3D_[point2D_idx] -= 1;
127 if (num_correspondences_have_point3D_[point2D_idx] == 0) {
128 num_visible_points3D_ -= 1;
129 }
130
131 point3D_visibility_pyramid_.ResetPoint(point2D.X(), point2D.Y());
132
133 assert(num_visible_points3D_ <= num_observations_);
134 }
135
NormalizeQvec()136 void Image::NormalizeQvec() { qvec_ = NormalizeQuaternion(qvec_); }
137
ProjectionMatrix() const138 Eigen::Matrix3x4d Image::ProjectionMatrix() const {
139 return ComposeProjectionMatrix(qvec_, tvec_);
140 }
141
InverseProjectionMatrix() const142 Eigen::Matrix3x4d Image::InverseProjectionMatrix() const {
143 return InvertProjectionMatrix(ComposeProjectionMatrix(qvec_, tvec_));
144 }
145
RotationMatrix() const146 Eigen::Matrix3d Image::RotationMatrix() const {
147 return QuaternionToRotationMatrix(qvec_);
148 }
149
ProjectionCenter() const150 Eigen::Vector3d Image::ProjectionCenter() const {
151 return ProjectionCenterFromPose(qvec_, tvec_);
152 }
153
ViewingDirection() const154 Eigen::Vector3d Image::ViewingDirection() const {
155 return RotationMatrix().row(2);
156 }
157
158 } // namespace colmap
159