src__base__cost_functions.h.patch

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_