1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Gael Guennebaud <g.gael (at) free.fr> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #include "main.h" 11 #include <unsupported/Eigen/AutoDiff> 12 13 template<typename Scalar> 14 EIGEN_DONT_INLINE Scalar foo(const Scalar& x, const Scalar& y) 15 { 16 using namespace std; 17 // return x+std::sin(y); 18 EIGEN_ASM_COMMENT("mybegin"); 19 // pow(float, int) promotes to pow(double, double) 20 return x*2 - 1 + static_cast<Scalar>(pow(1+x,2)) + 2*sqrt(y*y+0) - 4 * sin(0+x) + 2 * cos(y+0) - exp(Scalar(-0.5)*x*x+0); 21 //return x+2*y*x;//x*2 -std::pow(x,2);//(2*y/x);// - y*2; 22 EIGEN_ASM_COMMENT("myend"); 23 } 24 25 template<typename Vector> 26 EIGEN_DONT_INLINE typename Vector::Scalar foo(const Vector& p) 27 { 28 typedef typename Vector::Scalar Scalar; 29 return (p-Vector(Scalar(-1),Scalar(1.))).norm() + (p.array() * p.array()).sum() + p.dot(p); 30 } 31 32 template<typename _Scalar, int NX=Dynamic, int NY=Dynamic> 33 struct TestFunc1 34 { 35 typedef _Scalar Scalar; 36 enum { 37 InputsAtCompileTime = NX, 38 ValuesAtCompileTime = NY 39 }; 40 typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 41 typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 42 typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 43 44 int m_inputs, m_values; 45 46 TestFunc1() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 47 TestFunc1(int inputs, int values) : m_inputs(inputs), m_values(values) {} 48 49 int inputs() const { return m_inputs; } 50 int values() const { return m_values; } 51 52 template<typename T> 53 void operator() (const Matrix<T,InputsAtCompileTime,1>& x, Matrix<T,ValuesAtCompileTime,1>* _v) const 54 { 55 Matrix<T,ValuesAtCompileTime,1>& v = *_v; 56 57 v[0] = 2 * x[0] * x[0] + x[0] * x[1]; 58 v[1] = 3 * x[1] * x[0] + 0.5 * x[1] * x[1]; 59 if(inputs()>2) 60 { 61 v[0] += 0.5 * x[2]; 62 v[1] += x[2]; 63 } 64 if(values()>2) 65 { 66 v[2] = 3 * x[1] * x[0] * x[0]; 67 } 68 if (inputs()>2 && values()>2) 69 v[2] *= x[2]; 70 } 71 72 void operator() (const InputType& x, ValueType* v, JacobianType* _j) const 73 { 74 (*this)(x, v); 75 76 if(_j) 77 { 78 JacobianType& j = *_j; 79 80 j(0,0) = 4 * x[0] + x[1]; 81 j(1,0) = 3 * x[1]; 82 83 j(0,1) = x[0]; 84 j(1,1) = 3 * x[0] + 2 * 0.5 * x[1]; 85 86 if (inputs()>2) 87 { 88 j(0,2) = 0.5; 89 j(1,2) = 1; 90 } 91 if(values()>2) 92 { 93 j(2,0) = 3 * x[1] * 2 * x[0]; 94 j(2,1) = 3 * x[0] * x[0]; 95 } 96 if (inputs()>2 && values()>2) 97 { 98 j(2,0) *= x[2]; 99 j(2,1) *= x[2]; 100 101 j(2,2) = 3 * x[1] * x[0] * x[0]; 102 j(2,2) = 3 * x[1] * x[0] * x[0]; 103 } 104 } 105 } 106 }; 107 108 109 #if EIGEN_HAS_VARIADIC_TEMPLATES 110 /* Test functor for the C++11 features. */ 111 template <typename Scalar> 112 struct integratorFunctor 113 { 114 typedef Matrix<Scalar, 2, 1> InputType; 115 typedef Matrix<Scalar, 2, 1> ValueType; 116 117 /* 118 * Implementation starts here. 119 */ 120 integratorFunctor(const Scalar gain) : _gain(gain) {} 121 integratorFunctor(const integratorFunctor& f) : _gain(f._gain) {} 122 const Scalar _gain; 123 124 template <typename T1, typename T2> 125 void operator() (const T1 &input, T2 *output, const Scalar dt) const 126 { 127 T2 &o = *output; 128 129 /* Integrator to test the AD. */ 130 o[0] = input[0] + input[1] * dt * _gain; 131 o[1] = input[1] * _gain; 132 } 133 134 /* Only needed for the test */ 135 template <typename T1, typename T2, typename T3> 136 void operator() (const T1 &input, T2 *output, T3 *jacobian, const Scalar dt) const 137 { 138 T2 &o = *output; 139 140 /* Integrator to test the AD. */ 141 o[0] = input[0] + input[1] * dt * _gain; 142 o[1] = input[1] * _gain; 143 144 if (jacobian) 145 { 146 T3 &j = *jacobian; 147 148 j(0, 0) = 1; 149 j(0, 1) = dt * _gain; 150 j(1, 0) = 0; 151 j(1, 1) = _gain; 152 } 153 } 154 155 }; 156 157 template<typename Func> void forward_jacobian_cpp11(const Func& f) 158 { 159 typedef typename Func::ValueType::Scalar Scalar; 160 typedef typename Func::ValueType ValueType; 161 typedef typename Func::InputType InputType; 162 typedef typename AutoDiffJacobian<Func>::JacobianType JacobianType; 163 164 InputType x = InputType::Random(InputType::RowsAtCompileTime); 165 ValueType y, yref; 166 JacobianType j, jref; 167 168 const Scalar dt = internal::random<double>(); 169 170 jref.setZero(); 171 yref.setZero(); 172 f(x, &yref, &jref, dt); 173 174 //std::cerr << "y, yref, jref: " << "\n"; 175 //std::cerr << y.transpose() << "\n\n"; 176 //std::cerr << yref << "\n\n"; 177 //std::cerr << jref << "\n\n"; 178 179 AutoDiffJacobian<Func> autoj(f); 180 autoj(x, &y, &j, dt); 181 182 //std::cerr << "y j (via autodiff): " << "\n"; 183 //std::cerr << y.transpose() << "\n\n"; 184 //std::cerr << j << "\n\n"; 185 186 VERIFY_IS_APPROX(y, yref); 187 VERIFY_IS_APPROX(j, jref); 188 } 189 #endif 190 191 template<typename Func> void forward_jacobian(const Func& f) 192 { 193 typename Func::InputType x = Func::InputType::Random(f.inputs()); 194 typename Func::ValueType y(f.values()), yref(f.values()); 195 typename Func::JacobianType j(f.values(),f.inputs()), jref(f.values(),f.inputs()); 196 197 jref.setZero(); 198 yref.setZero(); 199 f(x,&yref,&jref); 200 // std::cerr << y.transpose() << "\n\n";; 201 // std::cerr << j << "\n\n";; 202 203 j.setZero(); 204 y.setZero(); 205 AutoDiffJacobian<Func> autoj(f); 206 autoj(x, &y, &j); 207 // std::cerr << y.transpose() << "\n\n";; 208 // std::cerr << j << "\n\n";; 209 210 VERIFY_IS_APPROX(y, yref); 211 VERIFY_IS_APPROX(j, jref); 212 } 213 214 // TODO also check actual derivatives! 215 template <int> 216 void test_autodiff_scalar() 217 { 218 Vector2f p = Vector2f::Random(); 219 typedef AutoDiffScalar<Vector2f> AD; 220 AD ax(p.x(),Vector2f::UnitX()); 221 AD ay(p.y(),Vector2f::UnitY()); 222 AD res = foo<AD>(ax,ay); 223 VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); 224 } 225 226 227 // TODO also check actual derivatives! 228 template <int> 229 void test_autodiff_vector() 230 { 231 Vector2f p = Vector2f::Random(); 232 typedef AutoDiffScalar<Vector2f> AD; 233 typedef Matrix<AD,2,1> VectorAD; 234 VectorAD ap = p.cast<AD>(); 235 ap.x().derivatives() = Vector2f::UnitX(); 236 ap.y().derivatives() = Vector2f::UnitY(); 237 238 AD res = foo<VectorAD>(ap); 239 VERIFY_IS_APPROX(res.value(), foo(p)); 240 } 241 242 template <int> 243 void test_autodiff_jacobian() 244 { 245 CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) )); 246 CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) )); 247 CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) )); 248 CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) )); 249 CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) )); 250 #if EIGEN_HAS_VARIADIC_TEMPLATES 251 CALL_SUBTEST(( forward_jacobian_cpp11(integratorFunctor<double>(10)) )); 252 #endif 253 } 254 255 256 template <int> 257 void test_autodiff_hessian() 258 { 259 typedef AutoDiffScalar<VectorXd> AD; 260 typedef Matrix<AD,Eigen::Dynamic,1> VectorAD; 261 typedef AutoDiffScalar<VectorAD> ADD; 262 typedef Matrix<ADD,Eigen::Dynamic,1> VectorADD; 263 VectorADD x(2); 264 double s1 = internal::random<double>(), s2 = internal::random<double>(), s3 = internal::random<double>(), s4 = internal::random<double>(); 265 x(0).value()=s1; 266 x(1).value()=s2; 267 268 //set unit vectors for the derivative directions (partial derivatives of the input vector) 269 x(0).derivatives().resize(2); 270 x(0).derivatives().setZero(); 271 x(0).derivatives()(0)= 1; 272 x(1).derivatives().resize(2); 273 x(1).derivatives().setZero(); 274 x(1).derivatives()(1)=1; 275 276 //repeat partial derivatives for the inner AutoDiffScalar 277 x(0).value().derivatives() = VectorXd::Unit(2,0); 278 x(1).value().derivatives() = VectorXd::Unit(2,1); 279 280 //set the hessian matrix to zero 281 for(int idx=0; idx<2; idx++) { 282 x(0).derivatives()(idx).derivatives() = VectorXd::Zero(2); 283 x(1).derivatives()(idx).derivatives() = VectorXd::Zero(2); 284 } 285 286 ADD y = sin(AD(s3)*x(0) + AD(s4)*x(1)); 287 288 VERIFY_IS_APPROX(y.value().derivatives()(0), y.derivatives()(0).value()); 289 VERIFY_IS_APPROX(y.value().derivatives()(1), y.derivatives()(1).value()); 290 VERIFY_IS_APPROX(y.value().derivatives()(0), s3*std::cos(s1*s3+s2*s4)); 291 VERIFY_IS_APPROX(y.value().derivatives()(1), s4*std::cos(s1*s3+s2*s4)); 292 VERIFY_IS_APPROX(y.derivatives()(0).derivatives(), -std::sin(s1*s3+s2*s4)*Vector2d(s3*s3,s4*s3)); 293 VERIFY_IS_APPROX(y.derivatives()(1).derivatives(), -std::sin(s1*s3+s2*s4)*Vector2d(s3*s4,s4*s4)); 294 295 ADD z = x(0)*x(1); 296 VERIFY_IS_APPROX(z.derivatives()(0).derivatives(), Vector2d(0,1)); 297 VERIFY_IS_APPROX(z.derivatives()(1).derivatives(), Vector2d(1,0)); 298 } 299 300 double bug_1222() { 301 typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD; 302 const double _cv1_3 = 1.0; 303 const AD chi_3 = 1.0; 304 // this line did not work, because operator+ returns ADS<DerType&>, which then cannot be converted to ADS<DerType> 305 const AD denom = chi_3 + _cv1_3; 306 return denom.value(); 307 } 308 309 double bug_1223() { 310 using std::min; 311 typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD; 312 313 const double _cv1_3 = 1.0; 314 const AD chi_3 = 1.0; 315 const AD denom = 1.0; 316 317 // failed because implementation of min attempts to construct ADS<DerType&> via constructor AutoDiffScalar(const Real& value) 318 // without initializing m_derivatives (which is a reference in this case) 319 #define EIGEN_TEST_SPACE 320 const AD t = min EIGEN_TEST_SPACE (denom / chi_3, 1.0); 321 322 const AD t2 = min EIGEN_TEST_SPACE (denom / (chi_3 * _cv1_3), 1.0); 323 324 return t.value() + t2.value(); 325 } 326 327 // regression test for some compilation issues with specializations of ScalarBinaryOpTraits 328 void bug_1260() { 329 Matrix4d A; 330 Vector4d v; 331 A*v; 332 } 333 334 // check a compilation issue with numext::max 335 double bug_1261() { 336 typedef AutoDiffScalar<Matrix2d> AD; 337 typedef Matrix<AD,2,1> VectorAD; 338 339 VectorAD v; 340 const AD maxVal = v.maxCoeff(); 341 const AD minVal = v.minCoeff(); 342 return maxVal.value() + minVal.value(); 343 } 344 345 double bug_1264() { 346 typedef AutoDiffScalar<Vector2d> AD; 347 const AD s; 348 const Matrix<AD, 3, 1> v1; 349 const Matrix<AD, 3, 1> v2 = (s + 3.0) * v1; 350 return v2(0).value(); 351 } 352 353 void test_autodiff() 354 { 355 for(int i = 0; i < g_repeat; i++) { 356 CALL_SUBTEST_1( test_autodiff_scalar<1>() ); 357 CALL_SUBTEST_2( test_autodiff_vector<1>() ); 358 CALL_SUBTEST_3( test_autodiff_jacobian<1>() ); 359 CALL_SUBTEST_4( test_autodiff_hessian<1>() ); 360 } 361 362 bug_1222(); 363 bug_1223(); 364 bug_1260(); 365 bug_1261(); 366 } 367 368
