-
Notifications
You must be signed in to change notification settings - Fork 0
/
src__base__cost_functions.h.patch
75 lines (73 loc) · 3.38 KB
/
src__base__cost_functions.h.patch
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
diff --git a/src/base/cost_functions.h b/src/base/cost_functions.h
index 7ed57d5..c1393bd 100755
--- a/src/base/cost_functions.h
+++ b/src/base/cost_functions.h
@@ -1,3 +1,35 @@
+// ===============================================================================================================
+// Copyright (c) 2019, Cornell University. All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+// the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright otice, this list of conditions and
+// the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
+// the following disclaimer in the documentation and/or other materials provided with the distribution.
+//
+// * Neither the name of Cornell University nor the names of its contributors may be used to endorse or
+// promote products derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
+// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+// OF SUCH DAMAGE.
+//
+// Author: Kai Zhang (kz298@cornell.edu)
+//
+// The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),
+// Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.
+// The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes.
+// ===============================================================================================================
+//
+//
// Copyright (c) 2022, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
@@ -267,6 +299,34 @@ class RelativePoseCostFunction {
const double y2_;
};
+// cost function that put a soft constraint on the 3D points
+// so that it does not deviate too much from the specified coordinates
+class Point3DCostFunction {
+ public:
+ explicit Point3DCostFunction(const Eigen::Vector3d& point3D)
+ : x_(point3D(0)), y_(point3D(1)), z_(point3D(2)) {}
+
+ static ceres::CostFunction* Create(const Eigen::Vector3d& point3D) {
+ return (new ceres::AutoDiffCostFunction<Point3DCostFunction, 3, 3>(
+ new Point3DCostFunction(point3D)));
+ }
+
+ template <typename T>
+ bool operator()(const T* const point3D, T* residuals) const {
+ // deviation error
+ residuals[0] = point3D[0] - T(x_);
+ residuals[1] = point3D[1] - T(y_);
+ residuals[2] = point3D[2] - T(z_);
+
+ return true;
+ }
+
+ private:
+ const double x_;
+ const double y_;
+ const double z_;
+};
+
} // namespace colmap
#endif // COLMAP_SRC_BASE_COST_FUNCTIONS_H_