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 #ifndef COLMAP_SRC_BASE_COST_FUNCTIONS_H_ 33 #define COLMAP_SRC_BASE_COST_FUNCTIONS_H_ 34 35 #include <Eigen/Core> 36 37 #include <ceres/ceres.h> 38 #include <ceres/rotation.h> 39 40 namespace colmap { 41 42 // Standard bundle adjustment cost function for variable 43 // camera pose and calibration and point parameters. 44 template <typename CameraModel> 45 class BundleAdjustmentCostFunction { 46 public: BundleAdjustmentCostFunction(const Eigen::Vector2d & point2D)47 explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) 48 : observed_x_(point2D(0)), observed_y_(point2D(1)) {} 49 Create(const Eigen::Vector2d & point2D)50 static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { 51 return (new ceres::AutoDiffCostFunction< 52 BundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 3, 53 CameraModel::kNumParams>( 54 new BundleAdjustmentCostFunction(point2D))); 55 } 56 57 template <typename T> operator()58 bool operator()(const T* const qvec, const T* const tvec, 59 const T* const point3D, const T* const camera_params, 60 T* residuals) const { 61 // Rotate and translate. 62 T projection[3]; 63 ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); 64 projection[0] += tvec[0]; 65 projection[1] += tvec[1]; 66 projection[2] += tvec[2]; 67 68 // Project to image plane. 69 projection[0] /= projection[2]; 70 projection[1] /= projection[2]; 71 72 // Distort and transform to pixel space. 73 CameraModel::WorldToImage(camera_params, projection[0], projection[1], 74 &residuals[0], &residuals[1]); 75 76 // Re-projection error. 77 residuals[0] -= T(observed_x_); 78 residuals[1] -= T(observed_y_); 79 80 return true; 81 } 82 83 private: 84 const double observed_x_; 85 const double observed_y_; 86 }; 87 88 // Bundle adjustment cost function for variable 89 // camera calibration and point parameters, and fixed camera pose. 90 template <typename CameraModel> 91 class BundleAdjustmentConstantPoseCostFunction { 92 public: BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d & qvec,const Eigen::Vector3d & tvec,const Eigen::Vector2d & point2D)93 BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec, 94 const Eigen::Vector3d& tvec, 95 const Eigen::Vector2d& point2D) 96 : qw_(qvec(0)), 97 qx_(qvec(1)), 98 qy_(qvec(2)), 99 qz_(qvec(3)), 100 tx_(tvec(0)), 101 ty_(tvec(1)), 102 tz_(tvec(2)), 103 observed_x_(point2D(0)), 104 observed_y_(point2D(1)) {} 105 Create(const Eigen::Vector4d & qvec,const Eigen::Vector3d & tvec,const Eigen::Vector2d & point2D)106 static ceres::CostFunction* Create(const Eigen::Vector4d& qvec, 107 const Eigen::Vector3d& tvec, 108 const Eigen::Vector2d& point2D) { 109 return (new ceres::AutoDiffCostFunction< 110 BundleAdjustmentConstantPoseCostFunction<CameraModel>, 2, 3, 111 CameraModel::kNumParams>( 112 new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D))); 113 } 114 115 template <typename T> operator()116 bool operator()(const T* const point3D, const T* const camera_params, 117 T* residuals) const { 118 const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)}; 119 120 // Rotate and translate. 121 T projection[3]; 122 ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); 123 projection[0] += T(tx_); 124 projection[1] += T(ty_); 125 projection[2] += T(tz_); 126 127 // Project to image plane. 128 projection[0] /= projection[2]; 129 projection[1] /= projection[2]; 130 131 // Distort and transform to pixel space. 132 CameraModel::WorldToImage(camera_params, projection[0], projection[1], 133 &residuals[0], &residuals[1]); 134 135 // Re-projection error. 136 residuals[0] -= T(observed_x_); 137 residuals[1] -= T(observed_y_); 138 139 return true; 140 } 141 142 private: 143 const double qw_; 144 const double qx_; 145 const double qy_; 146 const double qz_; 147 const double tx_; 148 const double ty_; 149 const double tz_; 150 const double observed_x_; 151 const double observed_y_; 152 }; 153 154 // Rig bundle adjustment cost function for variable camera pose and calibration 155 // and point parameters. Different from the standard bundle adjustment function, 156 // this cost function is suitable for camera rigs with consistent relative poses 157 // of the cameras within the rig. The cost function first projects points into 158 // the local system of the camera rig and then into the local system of the 159 // camera within the rig. 160 template <typename CameraModel> 161 class RigBundleAdjustmentCostFunction { 162 public: RigBundleAdjustmentCostFunction(const Eigen::Vector2d & point2D)163 explicit RigBundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) 164 : observed_x_(point2D(0)), observed_y_(point2D(1)) {} 165 Create(const Eigen::Vector2d & point2D)166 static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { 167 return (new ceres::AutoDiffCostFunction< 168 RigBundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 4, 3, 3, 169 CameraModel::kNumParams>( 170 new RigBundleAdjustmentCostFunction(point2D))); 171 } 172 173 template <typename T> operator()174 bool operator()(const T* const rig_qvec, const T* const rig_tvec, 175 const T* const rel_qvec, const T* const rel_tvec, 176 const T* const point3D, const T* const camera_params, 177 T* residuals) const { 178 // Concatenate rotations. 179 T qvec[4]; 180 ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec); 181 182 // Concatenate translations. 183 T tvec[3]; 184 ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec); 185 tvec[0] += rel_tvec[0]; 186 tvec[1] += rel_tvec[1]; 187 tvec[2] += rel_tvec[2]; 188 189 // Rotate and translate. 190 T projection[3]; 191 ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); 192 projection[0] += tvec[0]; 193 projection[1] += tvec[1]; 194 projection[2] += tvec[2]; 195 196 // Project to image plane. 197 projection[0] /= projection[2]; 198 projection[1] /= projection[2]; 199 200 // Distort and transform to pixel space. 201 CameraModel::WorldToImage(camera_params, projection[0], projection[1], 202 &residuals[0], &residuals[1]); 203 204 // Re-projection error. 205 residuals[0] -= T(observed_x_); 206 residuals[1] -= T(observed_y_); 207 208 return true; 209 } 210 211 private: 212 const double observed_x_; 213 const double observed_y_; 214 }; 215 216 // Cost function for refining two-view geometry based on the Sampson-Error. 217 // 218 // First pose is assumed to be located at the origin with 0 rotation. Second 219 // pose is assumed to be on the unit sphere around the first pose, i.e. the 220 // pose of the second camera is parameterized by a 3D rotation and a 221 // 3D translation with unit norm. `tvec` is therefore over-parameterized as is 222 // and should be down-projected using `HomogeneousVectorParameterization`. 223 class RelativePoseCostFunction { 224 public: RelativePoseCostFunction(const Eigen::Vector2d & x1,const Eigen::Vector2d & x2)225 RelativePoseCostFunction(const Eigen::Vector2d& x1, const Eigen::Vector2d& x2) 226 : x1_(x1(0)), y1_(x1(1)), x2_(x2(0)), y2_(x2(1)) {} 227 Create(const Eigen::Vector2d & x1,const Eigen::Vector2d & x2)228 static ceres::CostFunction* Create(const Eigen::Vector2d& x1, 229 const Eigen::Vector2d& x2) { 230 return (new ceres::AutoDiffCostFunction<RelativePoseCostFunction, 1, 4, 3>( 231 new RelativePoseCostFunction(x1, x2))); 232 } 233 234 template <typename T> operator()235 bool operator()(const T* const qvec, const T* const tvec, 236 T* residuals) const { 237 Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R; 238 ceres::QuaternionToRotation(qvec, R.data()); 239 240 // Matrix representation of the cross product t x R. 241 Eigen::Matrix<T, 3, 3> t_x; 242 t_x << T(0), -tvec[2], tvec[1], tvec[2], T(0), -tvec[0], -tvec[1], tvec[0], 243 T(0); 244 245 // Essential matrix. 246 const Eigen::Matrix<T, 3, 3> E = t_x * R; 247 248 // Homogeneous image coordinates. 249 const Eigen::Matrix<T, 3, 1> x1_h(T(x1_), T(y1_), T(1)); 250 const Eigen::Matrix<T, 3, 1> x2_h(T(x2_), T(y2_), T(1)); 251 252 // Squared sampson error. 253 const Eigen::Matrix<T, 3, 1> Ex1 = E * x1_h; 254 const Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * x2_h; 255 const T x2tEx1 = x2_h.transpose() * Ex1; 256 residuals[0] = x2tEx1 * x2tEx1 / 257 (Ex1(0) * Ex1(0) + Ex1(1) * Ex1(1) + Etx2(0) * Etx2(0) + 258 Etx2(1) * Etx2(1)); 259 260 return true; 261 } 262 263 private: 264 const double x1_; 265 const double y1_; 266 const double x2_; 267 const double y2_; 268 }; 269 270 } // namespace colmap 271 272 #endif // COLMAP_SRC_BASE_COST_FUNCTIONS_H_ 273