Eigen arctan2

Ref

https://forum.kde.org/viewtopic.php?t=112142

Code

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;

double my_atan2(double a, double b) { return std::atan2(a,b); }

int main()
{
  Matrix4d A, B, C;
  C = A.binaryExpr(B, std::ptr_fun(::atan2));
  C = A.binaryExpr(B, std::ptr_fun(my_atan2));
  C = A.binaryExpr(B, [] (double a, double b) { return std::atan2(a,b);} );
}

test by myself

a c++ file

	cout<<"yaw = "<<y.binaryExpr(x, std::ptr_fun(my_atan2))<<endl;
	//yaw =   1.5708
	//		0.896055
	//		0.844154

in python file

yaw = -np.arctan2(y, x)
# yaw in numpy =  
# [-1.57079633 -0.89605538 -0.84415399]