├── LICENSE ├── README.md └── ceres_extensions.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 Lloyd Hughes 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ceres_extensions 2 | ================ 3 | 4 | Some extensions to make ceres solver work a little better with Eigen. 5 | 6 | This includes: 7 | * Eigen Quaternion parameterizations 8 | * Eigen Quaternion rotations 9 | * Eigen Quaternion multiplications 10 | 11 | To use the extensions just reference ceres_ext namespace and append the normal name with Eigen. 12 | 13 | ie: 14 | 15 | ceres::QuaternionParameterization becomes ceres_ext::EigenQuaternionParameterization 16 | 17 | I might consider creating a pull request against ceres solver but I am too lazy to do it now. 18 | -------------------------------------------------------------------------------- /ceres_extensions.h: -------------------------------------------------------------------------------- 1 | // 2 | // ceres_extensions.h 3 | // Bundle_Adjust_Test 4 | // 5 | // Created by Lloyd Hughes on 2014/04/11. 6 | // Copyright (c) 2014 Lloyd Hughes. All rights reserved. 7 | // hughes.lloyd@gmail.com 8 | // 9 | 10 | #ifndef Bundle_Adjust_Test_ceres_extensions_h 11 | #define Bundle_Adjust_Test_ceres_extensions_h 12 | 13 | #include "Eigen/Core" 14 | #include 15 | #include 16 | 17 | namespace ceres_ext { 18 | 19 | // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 20 | // with * being the quaternion multiplication operator. Here we assume 21 | // that the first element of the quaternion vector is the real (cos 22 | // theta) part. 23 | class EigenQuaternionParameterization : public ceres::LocalParameterization { 24 | public: 25 | virtual ~EigenQuaternionParameterization() {} 26 | 27 | virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const { 28 | const Eigen::Map x(x_raw); 29 | const Eigen::Map delta(delta_raw); 30 | 31 | Eigen::Map x_plus_delta(x_plus_delta_raw); 32 | 33 | const double delta_norm = delta.norm(); 34 | if ( delta_norm > 0.0 ){ 35 | const double sin_delta_by_delta = sin(delta_norm) / delta_norm; 36 | Eigen::Quaterniond tmp( cos(delta_norm), sin_delta_by_delta*delta[0], sin_delta_by_delta*delta[1], sin_delta_by_delta*delta[2] ); 37 | 38 | x_plus_delta = tmp*x; 39 | } 40 | else { 41 | x_plus_delta = x; 42 | } 43 | return true; 44 | } 45 | 46 | virtual bool ComputeJacobian(const double* x, double* jacobian) const { 47 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT x 48 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT y 49 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT z 50 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT w 51 | return true; 52 | } 53 | 54 | virtual int GlobalSize() const { return 4; } 55 | virtual int LocalSize() const { return 3; } 56 | 57 | }; 58 | 59 | template inline 60 | void EigenQuaternionToScaledRotation(const T q[4], T R[3 * 3]) { 61 | EigenQuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); 62 | } 63 | 64 | template inline 65 | void EigenQuaternionToScaledRotation(const T q[4], 66 | const ceres::MatrixAdapter& R) { 67 | // Make convenient names for elements of q. 68 | T a = q[3]; 69 | T b = q[0]; 70 | T c = q[1]; 71 | T d = q[2]; 72 | // This is not to eliminate common sub-expression, but to 73 | // make the lines shorter so that they fit in 80 columns! 74 | T aa = a * a; 75 | T ab = a * b; 76 | T ac = a * c; 77 | T ad = a * d; 78 | T bb = b * b; 79 | T bc = b * c; 80 | T bd = b * d; 81 | T cc = c * c; 82 | T cd = c * d; 83 | T dd = d * d; 84 | 85 | R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT 86 | R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT 87 | R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT 88 | } 89 | 90 | template inline 91 | void EigenQuaternionToRotation(const T q[4], T R[3 * 3]) { 92 | EigenQuaternionToRotation(q, RowMajorAdapter3x3(R)); 93 | } 94 | 95 | template inline 96 | void EigenQuaternionToRotation(const T q[4], 97 | const ceres::MatrixAdapter& R) { 98 | EigenQuaternionToScaledRotation(q, R); 99 | 100 | T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; 101 | CHECK_NE(normalizer, T(0)); 102 | normalizer = T(1) / normalizer; 103 | 104 | for (int i = 0; i < 3; ++i) { 105 | for (int j = 0; j < 3; ++j) { 106 | R(i, j) *= normalizer; 107 | } 108 | } 109 | } 110 | 111 | template inline 112 | void EigenUnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 113 | const T t2 = q[3] * q[0]; 114 | const T t3 = q[3] * q[1]; 115 | const T t4 = q[3] * q[2]; 116 | const T t5 = -q[0] * q[0]; 117 | const T t6 = q[0] * q[1]; 118 | const T t7 = q[0] * q[2]; 119 | const T t8 = -q[1] * q[1]; 120 | const T t9 = q[1] * q[2]; 121 | const T t1 = -q[2] * q[2]; 122 | result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT 123 | result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT 124 | result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT 125 | } 126 | 127 | template inline 128 | void EigenQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 129 | // 'scale' is 1 / norm(q). 130 | const T scale = T(1) / sqrt(q[0] * q[0] + 131 | q[1] * q[1] + 132 | q[2] * q[2] + 133 | q[3] * q[3]); 134 | 135 | // Make unit-norm version of q. 136 | const T unit[4] = { 137 | scale * q[0], 138 | scale * q[1], 139 | scale * q[2], 140 | scale * q[3], 141 | }; 142 | 143 | EigenUnitQuaternionRotatePoint(unit, pt, result); 144 | } 145 | 146 | template inline 147 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) { 148 | zw[0] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; 149 | zw[1] = - z[0] * w[2] + z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; 150 | zw[2] = z[0] * w[1] - z[1] * w[0] + z[2] * w[3] + z[3] * w[2]; 151 | zw[3] = - z[0] * w[0] - z[1] * w[1] - z[2] * w[2] + z[3] * w[3]; 152 | } 153 | 154 | } 155 | 156 | #endif 157 | --------------------------------------------------------------------------------