TEST(AgradFwdAcos,FvarFvarVar_2ndDeriv) {
using stan::agrad::fvar;
using stan::agrad::var;
using std::acos;
fvar<fvar<var> > z;
z.val_.val_ = 0.5;
z.val_.d_ = 2.0;
fvar<fvar<var> > b = acos(z);
AVEC y = createAVEC(z.val_.val_);
VEC g;
b.val_.d_.grad(y,g);
EXPECT_FLOAT_EQ(-0.5 * 2.0 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), g[0]);
fvar<fvar<var> > w;
w.val_.val_ = 0.5;
w.d_.val_ = 2.0;
fvar<fvar<var> > c = acos(w);
AVEC p = createAVEC(w.val_.val_);
VEC q;
c.d_.val_.grad(p,q);
EXPECT_FLOAT_EQ(-0.5 * 2.0 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), q[0]);
}
TEST(AgradFwdAcos,FvarFvarDouble) {
using stan::math::fvar;
using std::acos;
fvar<fvar<double> > x;
x.val_.val_ = 0.5;
x.val_.d_ = 2.0;
fvar<fvar<double> > a = acos(x);
EXPECT_FLOAT_EQ(acos(0.5), a.val_.val_);
EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), a.val_.d_);
EXPECT_FLOAT_EQ(0, a.d_.val_);
EXPECT_FLOAT_EQ(0, a.d_.d_);
fvar<fvar<double> > y;
y.val_.val_ = 0.5;
y.d_.val_ = 2.0;
a = acos(y);
EXPECT_FLOAT_EQ(acos(0.5), a.val_.val_);
EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), a.d_.val_);
EXPECT_FLOAT_EQ(0, a.val_.d_);
EXPECT_FLOAT_EQ(0, a.d_.d_);
}
template<typename T> T bounded_acos(T v)
{
using std::acos;
using std::min;
using std::max;
return acos((max)(T(-1),(min)(v,T(1))));
}
TEST(AgradFwdAcos,FvarVar_1stDeriv) {
using stan::agrad::fvar;
using stan::agrad::var;
using std::acos;
fvar<var> x(0.5,0.3);
fvar<var> a = acos(x);
EXPECT_FLOAT_EQ(acos(0.5), a.val_.val());
EXPECT_FLOAT_EQ(-0.3 / sqrt(1.0 - 0.5 * 0.5), a.d_.val());
AVEC y = createAVEC(x.val_);
VEC g;
a.val_.grad(y,g);
EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]);
}
void VKDrone::aimAt(double x, double y) {
// Angle from NORTH to "ang" attribute restricted to [-2PI, 2PI].
double facingAngle = fmod(myShip->ang, 360) * (PI / 180.0);
// Ajust facingAngle to [-PI, PI]. West -> negative, East -> positive.
if (facingAngle > PI) {
facingAngle -= 2 * PI;
} else if (facingAngle < -PI) {
facingAngle += 2 * PI;
}
// Unitary vector of facing direction from NORTH.
double facingX = -1 * sin(facingAngle);
double facingY = cos(facingAngle);
// Vector to target with respect to myShip.
double targetX = x - myShip->posx;
double targetY = y - myShip->posy;
// Find the angle [0,PI] between the vectors using "dot (or inner) product".
double dotProduct = facingX * targetX + facingY * targetY;
double norm = sqrt(facingX * facingX + facingY * facingY) *
sqrt(targetX * targetX + targetY * targetY);
double result = max(min(dotProduct / norm, 1.0), -1.0);
double angleToTarget = acos(result);
// Cross product sign tells if target is at right (negative angleToTarget).
if (facingX * targetY - facingY * targetX < 0) {
angleToTarget *= -1.0;
}
sideThrustFront = rotationPID.compute(0.0, angleToTarget);
sideThrustBack = -sideThrustFront;
}
void complex_number_examples()
{
Complex z1{0, 1};
std::cout << std::setprecision(std::numeric_limits<typename Complex::value_type>::digits10);
std::cout << std::scientific << std::fixed;
std::cout << "Print a complex number: " << z1 << std::endl;
std::cout << "Square it : " << z1*z1 << std::endl;
std::cout << "Real part : " << z1.real() << " = " << real(z1) << std::endl;
std::cout << "Imaginary part : " << z1.imag() << " = " << imag(z1) << std::endl;
using std::abs;
std::cout << "Absolute value : " << abs(z1) << std::endl;
std::cout << "Argument : " << arg(z1) << std::endl;
std::cout << "Norm : " << norm(z1) << std::endl;
std::cout << "Complex conjugate : " << conj(z1) << std::endl;
std::cout << "Projection onto Riemann sphere: " << proj(z1) << std::endl;
typename Complex::value_type r = 1;
typename Complex::value_type theta = 0.8;
using std::polar;
std::cout << "Polar coordinates (phase = 0) : " << polar(r) << std::endl;
std::cout << "Polar coordinates (phase !=0) : " << polar(r, theta) << std::endl;
std::cout << "\nElementary special functions:\n";
using std::exp;
std::cout << "exp(z1) = " << exp(z1) << std::endl;
using std::log;
std::cout << "log(z1) = " << log(z1) << std::endl;
using std::log10;
std::cout << "log10(z1) = " << log10(z1) << std::endl;
using std::pow;
std::cout << "pow(z1, z1) = " << pow(z1, z1) << std::endl;
using std::sqrt;
std::cout << "Take its square root : " << sqrt(z1) << std::endl;
using std::sin;
std::cout << "sin(z1) = " << sin(z1) << std::endl;
using std::cos;
std::cout << "cos(z1) = " << cos(z1) << std::endl;
using std::tan;
std::cout << "tan(z1) = " << tan(z1) << std::endl;
using std::asin;
std::cout << "asin(z1) = " << asin(z1) << std::endl;
using std::acos;
std::cout << "acos(z1) = " << acos(z1) << std::endl;
using std::atan;
std::cout << "atan(z1) = " << atan(z1) << std::endl;
using std::sinh;
std::cout << "sinh(z1) = " << sinh(z1) << std::endl;
using std::cosh;
std::cout << "cosh(z1) = " << cosh(z1) << std::endl;
using std::tanh;
std::cout << "tanh(z1) = " << tanh(z1) << std::endl;
using std::asinh;
std::cout << "asinh(z1) = " << asinh(z1) << std::endl;
using std::acosh;
std::cout << "acosh(z1) = " << acosh(z1) << std::endl;
using std::atanh;
std::cout << "atanh(z1) = " << atanh(z1) << std::endl;
}
inline
fvar<T>
acos(const fvar<T>& x) {
using std::acos;
using std::sqrt;
using stan::math::square;
return fvar<T>(acos(x.val_), x.d_ / -sqrt(1 - square(x.val_)));
}
/** Cartesian to spherical coordinates */
inline static
void cart2sph(real_type& r, real_type& theta, real_type& phi,
const point_type& x) {
using std::acos;
using std::atan2;
r = norm_2(x);
theta = acos(x[2] / (r + 1e-100));
phi = atan2(x[1], x[0]);
}
TEST(AgradFwdAcos,FvarVar_2ndDeriv) {
using stan::agrad::fvar;
using stan::agrad::var;
using std::acos;
fvar<var> x(0.5,0.3);
fvar<var> a = acos(x);
AVEC z = createAVEC(x.val_);
VEC h;
a.d_.grad(z,h);
EXPECT_FLOAT_EQ(-0.5 * 0.3 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), h[0]);
}
TEST(AgradFwdAcos,FvarFvarVar_1stDeriv) {
using stan::math::fvar;
using stan::math::var;
using std::acos;
fvar<fvar<var> > z;
z.val_.val_ = 0.5;
z.val_.d_ = 2.0;
fvar<fvar<var> > b = acos(z);
EXPECT_FLOAT_EQ(acos(0.5), b.val_.val_.val());
EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), b.val_.d_.val());
EXPECT_FLOAT_EQ(0, b.d_.val_.val());
EXPECT_FLOAT_EQ(0, b.d_.d_.val());
AVEC y = createAVEC(z.val_.val_);
VEC g;
b.val_.val_.grad(y,g);
EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]);
fvar<fvar<var> > w;
w.val_.val_ = 0.5;
w.d_.val_ = 2.0;
b = acos(w);
EXPECT_FLOAT_EQ(acos(0.5), b.val_.val_.val());
EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), b.d_.val_.val());
EXPECT_FLOAT_EQ(0, b.val_.d_.val());
EXPECT_FLOAT_EQ(0, b.d_.d_.val());
AVEC p = createAVEC(w.val_.val_);
VEC q;
b.val_.val_.grad(p,q);
EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), q[0]);
}
TEST(AgradFwdAcos,FvarFvarVar_3rdDeriv) {
using stan::agrad::fvar;
using stan::agrad::var;
using std::acos;
fvar<fvar<var> > z;
z.val_.val_ = 0.5;
z.val_.d_ = 1.0;
z.d_.val_ = 1.0;
fvar<fvar<var> > b = acos(z);
AVEC y = createAVEC(z.val_.val_);
VEC g;
b.d_.d_.grad(y,g);
EXPECT_FLOAT_EQ(-3.07920143567800, g[0]);
}
void Compensator::motion_compensation(sensor_msgs::PointCloud2::Ptr& msg,
const double timestamp_min,
const double timestamp_max,
const Eigen::Affine3d& pose_min_time,
const Eigen::Affine3d& pose_max_time) {
using std::abs;
using std::sin;
using std::acos;
Eigen::Vector3d translation =
pose_min_time.translation() - pose_max_time.translation();
Eigen::Quaterniond q_max(pose_max_time.linear());
Eigen::Quaterniond q_min(pose_min_time.linear());
Eigen::Quaterniond q1(q_max.conjugate() * q_min);
Eigen::Quaterniond q0(Eigen::Quaterniond::Identity());
q1.normalize();
translation = q_max.conjugate() * translation;
int total = msg->width * msg->height;
double d = q0.dot(q1);
double abs_d = abs(d);
double f = 1.0 / (timestamp_max - timestamp_min);
// Threshold for a "significant" rotation from min_time to max_time:
// The LiDAR range accuracy is ~2 cm. Over 70 meters range, it means an angle
// of 0.02 / 70 =
// 0.0003 rad. So, we consider a rotation "significant" only if the scalar
// part of quaternion is
// less than cos(0.0003 / 2) = 1 - 1e-8.
if (abs_d < 1.0 - 1.0e-8) {
double theta = acos(abs_d);
double sin_theta = sin(theta);
double c1_sign = (d > 0) ? 1 : -1;
for (int i = 0; i < total; ++i) {
size_t offset = i * msg->point_step;
Scalar* x_scalar =
reinterpret_cast<Scalar*>(&msg->data[offset + x_offset_]);
if (std::isnan(*x_scalar)) {
ROS_DEBUG_STREAM("nan point do not need motion compensation");
continue;
}
Scalar* y_scalar =
reinterpret_cast<Scalar*>(&msg->data[offset + y_offset_]);
Scalar* z_scalar =
reinterpret_cast<Scalar*>(&msg->data[offset + z_offset_]);
Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);
double tp = 0.0;
memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
timestamp_data_size_);
double t = (timestamp_max - tp) * f;
Eigen::Translation3d ti(t * translation);
double c0 = sin((1 - t) * theta) / sin_theta;
double c1 = sin(t * theta) / sin_theta * c1_sign;
Eigen::Quaterniond qi(c0 * q0.coeffs() + c1 * q1.coeffs());
Eigen::Affine3d trans = ti * qi;
p = trans * p;
*x_scalar = p.x();
*y_scalar = p.y();
*z_scalar = p.z();
}
return;
}
// Not a "significant" rotation. Do translation only.
for (int i = 0; i < total; ++i) {
Scalar* x_scalar =
reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + x_offset_]);
if (std::isnan(*x_scalar)) {
ROS_DEBUG_STREAM("nan point do not need motion compensation");
continue;
}
Scalar* y_scalar =
reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + y_offset_]);
Scalar* z_scalar =
reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + z_offset_]);
Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);
double tp = 0.0;
memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
timestamp_data_size_);
double t = (timestamp_max - tp) * f;
Eigen::Translation3d ti(t * translation);
p = ti * p;
*x_scalar = p.x();
*y_scalar = p.y();
*z_scalar = p.z();
}
}
inline T0
operator()(const T0& arg1) const {
return acos(arg1);
}
/** Vector Angle
*
* cos <a> = (a * b) / (|a|*|b|)
*/
double angle(const Point& p1, const Point& p2, const Point& p3){
Segment s1 = Segment(p2,p1);
Segment s2 = Segment(p2,p3);
return acos(scalar_product(s1,s2) / (s2.magnitude() * s1.magnitude()));
}
TEST(AgradFwdAcos,Fvar) {
using stan::math::fvar;
using std::acos;
using std::sqrt;
using std::isnan;
using stan::math::NEGATIVE_INFTY;
fvar<double> x(0.5,1.0);
fvar<double> a = acos(x);
EXPECT_FLOAT_EQ(acos(0.5), a.val_);
EXPECT_FLOAT_EQ(1 / -sqrt(1 - 0.5 * 0.5), a.d_);
fvar<double> b = 2 * acos(x) + 4;
EXPECT_FLOAT_EQ(2 * acos(0.5) + 4, b.val_);
EXPECT_FLOAT_EQ(2 / -sqrt(1 - 0.5 * 0.5), b.d_);
fvar<double> c = -acos(x) + 5;
EXPECT_FLOAT_EQ(-acos(0.5) + 5, c.val_);
EXPECT_FLOAT_EQ(-1 / -sqrt(1 - 0.5 * 0.5), c.d_);
fvar<double> d = -3 * acos(x) + 5 * x;
EXPECT_FLOAT_EQ(-3 * acos(0.5) + 5 * 0.5, d.val_);
EXPECT_FLOAT_EQ(-3 / -sqrt(1 - 0.5 * 0.5) + 5, d.d_);
fvar<double> y(3.4);
y.d_ = 1.0;
fvar<double> e = acos(y);
isnan(e.val_);
isnan(e.d_);
fvar<double> z(1.0);
z.d_ = 1.0;
fvar<double> f = acos(z);
EXPECT_FLOAT_EQ(acos(1.0), f.val_);
EXPECT_FLOAT_EQ(NEGATIVE_INFTY, f.d_);
fvar<double> z2(1.0+stan::math::EPSILON,1.0);
fvar<double> f2 = acos(z2);
EXPECT_TRUE(boost::math::isnan(f2.val_));
EXPECT_TRUE(boost::math::isnan(f2.d_));
fvar<double> z3(-1.0-stan::math::EPSILON,1.0);
fvar<double> f3 = acos(z3);
EXPECT_TRUE(boost::math::isnan(f3.val_));
EXPECT_TRUE(boost::math::isnan(f3.d_));
}
