/**
* @brief Extract angles corresponding to an YXZ transformation
* @param angles Resulting angles, in order: phi,theta,psi about Y,X,Z
*
* @note In the case where cos(phi)==0, we set psi = 0 (gimbal lock).
*/
void extractYXZ(float angles[3]) const
{
static_assert(N == 3 && M == 3, "Matrix must be a member of SO3 group");
using std::cos;
using std::atan2;
const matrix <3,3>& R = *this;
// numerical rounding may cause this value to drift out of bounds
float nsin_phi = R(1,2);
if (nsin_phi < -1.0f) {
nsin_phi = -1.0f;
} else if (nsin_phi > 1.0f) {
nsin_phi = 1.0f;
}
angles[0] = asinf( -nsin_phi ); // phi
if (cos(angles[0]) < 1.0e-5f)
{
angles[1] = atan2(R(0,1), R(0,0)); // theta
angles[2] = 0.0f; // psi
}
else
{
angles[1] = atan2(R(0,2), R(2,2)); // theta
angles[2] = atan2(R(1,0), R(1,1)); // psi
}
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
controlSystem = controller->getControlSystem();
systemUnlocked = controller->getSystemStatus();
if (controlSystem==2 && systemUnlocked){
roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18);
controller->setRoll(roll_w);
} else {
pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
controller->setPitch(pitch_w);
//cout << "SPEED: " << controller->longSpeed[pitch_w] << endl;
}
if (controlSystem == 0 && systemUnlocked){
yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
controller->setYaw(yaw_w);
}
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
//hacky globals
ROLL = roll;
PITCH = pitch;
YAW = yaw;
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
if (CURRENTPOSE == myo::Pose::waveIn) {
WAVEINCOUNTER += 2;
}
if (CURRENTPOSE == myo::Pose::fingersSpread) {
FINGSERSPREADCOUNTER += 2;
}
if (WAVEINCOUNTER != 1) {
WAVEINCOUNTER -= 1;
}
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
std::cout<<"orientation fucntion called"<<std::endl;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
p << osc::BeginMessage("/myo/orientation")
<< MAC
<< quat.x() << quat.y() << quat.z() << quat.w() << roll << pitch << yaw << osc::EndMessage;
transmitSocket->Send(p.Data(), p.Size());
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
}
/*!
* Convert X, Y, Z in wgs84 to longitude, latitude, height
* @param[in] x in meter
* @param[in] y in meter
* @param[in] z in meter
* @return llh vector<double> contains longitude, latitude, height
*/
vector<double> ecef2llh(const double& x, const double& y, const double& z)
{
double longitude {0.0}; /*< longitude in radian */
double latitude {0.0}; /*< latitude in radian */
double height {0.0}; /*< height in meter */
double p = sqrt((x * x) + (y * y));
double theta = atan2((z * A), (p * B));
/* Avoid 0 division error */
if(x == 0.0 && y == 0.0) {
vector<double>
llh {0.0, copysign(90.0,z), z - copysign(B,z)};
return llh;
} else {
latitude = atan2(
(z + (Er * Er * B * pow(sin(theta), 3))),
(p - (E * E * A * pow(cos(theta), 3)))
);
longitude = atan2(y, x);
double n = A / sqrt(1.0 - E * E * sin(latitude) * sin(latitude));
height = p / cos(latitude) - n;
vector<double>
llh {toDeg(longitude), toDeg(latitude), height};
return llh;
}
}
TEST(AgradFwdAtan2,FvarFvarDouble) {
using stan::math::fvar;
using std::atan2;
fvar<fvar<double> > x;
x.val_.val_ = 1.5;
x.val_.d_ = 1.0;
fvar<fvar<double> > y;
y.val_.val_ = 1.5;
y.d_.val_ = 1.0;
double z = 1.5;
fvar<fvar<double> > a = atan2(x,y);
EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
EXPECT_FLOAT_EQ(1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.val_.d_);
EXPECT_FLOAT_EQ(-1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.d_.val_);
EXPECT_FLOAT_EQ((1.5 * 1.5 - 1.5 * 1.5) / ((1.5 * 1.5 + 1.5 * 1.5) * (1.5 * 1.5 + 1.5 * 1.5)), a.d_.d_);
a = atan2(x,z);
EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
EXPECT_FLOAT_EQ(1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.val_.d_);
EXPECT_FLOAT_EQ(0.0, a.d_.val_);
EXPECT_FLOAT_EQ(0.0, a.d_.d_);
a = atan2(z, y);
EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
EXPECT_FLOAT_EQ(0.0, a.val_.d_);
EXPECT_FLOAT_EQ(-1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.d_.val_);
EXPECT_FLOAT_EQ(0.0, a.d_.d_);
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
float roll_f = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
float pitch_f = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
float yaw_f = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
int roll_w = static_cast<int>(roll_f);
int pitch_w = static_cast<int>(pitch_f);
int yaw_w = static_cast<int>(yaw_f);
MyoData &data = knownMyosData[myo];
data.roll = roll_f/18.0*100.0;
data.pitch = pitch_f/18.0*100.0;
data.yaw = yaw_f/18.0*100.0;
sendOrientation(myo, data.roll, data.pitch, data.yaw);
}
void DeviceCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
Device * device = findDevice(myo);
if ( device ) {
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
device->q = quat;//set(quat.x(), quat.y(), quat.z(), quat.w());
device->roll = roll;
device->pitch = pitch;
device->yaw = yaw;
// Convert the floating point angles in radians to a scale from 0 to 20.
device->roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
device->pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
device->yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
float gyoro_g = sqrt(device->gyro.x()*device->gyro.x() + device->gyro.y()*device->gyro.y() + device->gyro.z()*device->gyro.z());
float g = sqrt(device->accel.x()*device->accel.x() + device->accel.y()*device->accel.y() + device->accel.z()*device->accel.z());
// cout << gyoro_g << endl;
// cout << g << endl;
if ( gyoro_g <= 0.2 ) device->gravity = g;
// Quaternion4f q;
// q = quat;
// //.set(quat.x(), quat.z(), quat.y(), quat.w());
// Vector3f linear_accel;
// ofMatrix4x4 mat;
// mat.translate(ofVec3f(0,device->gravity,0));
// mat.rotate(q);
// ofVec3f trans = mat.getTranslation();
//
// linear_accel = device->getAccel();
// linear_accel.x = linear_accel.x - trans.x;
// linear_accel.y = linear_accel.y - trans.z;
// linear_accel.z = linear_accel.z - trans.y;
//
// device->linear_accel.set(linear_accel);
// cout << device->getAccel() << endl;
// cout << mat.getTranslation() << endl;
// cout << linear_accel << endl;
}
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
float drop = 1.1;
roll = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
if(pitch > 10)
{ if(pitch < 20)
{ pitch *= (drop*(20-pitch));
}
}
else
{ if(pitch != 10)
{ pitch *= (drop*(10-pitch));
}
}
yaw = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
if(yaw > 10.0000)
{ if(yaw < 20.0000)
{ yaw *= (drop*(20-yaw));
}
else
{ yaw = 19.9999;
}
}
else
{ if(yaw != 10.0000)
{ yaw *= (drop*(10-yaw));
}
}
if(color_input == true)
{ float x = (GetSystemMetrics(SM_CXSCREEN));
float y = (GetSystemMetrics(SM_CYSCREEN));
SetCursorPos((((((20)-yaw))/(20))*x), ((pitch/(20))*y));
}
}
typename stan::return_type<T_y, T_loc, T_scale>::type
cdf_function(const T_y& y, const T_loc& mu, const T_scale& sigma,
const T3&, const T4&, const T5&) {
using std::atan2;
using stan::math::pi;
return atan2(y-mu, sigma) / pi() + 0.5;
}
inline
typename quan::where_<
quan::meta::and_<
quan::meta::dimensionally_equivalent<StaticUnit_L, StaticUnit_R>,
quan::meta::or_<
quan::meta::and_<
quan::meta::allow_implicit_unit_conversions<StaticUnit_L>,
quan::meta::allow_implicit_unit_conversions<StaticUnit_R>
>,
quan::meta::is_math_same_conversion_factor<
typename quan::meta::get_conversion_factor<StaticUnit_L>::type,
typename quan::meta::get_conversion_factor<StaticUnit_R>::type
>
>
>,
typename quan::angle_<
typename quan::meta::binary_op<
fixed_quantity<StaticUnit_L, NumericType_L>,
quan::meta::divides,
fixed_quantity<StaticUnit_R, NumericType_R>
>::type
>::rad
>::type
atan2(fixed_quantity<StaticUnit_L, NumericType_L> const & y,
fixed_quantity<StaticUnit_R, NumericType_R> const & x)
{
typedef fixed_quantity<StaticUnit_L, NumericType_L> arg_type1;
typedef fixed_quantity<StaticUnit_R, NumericType_R> arg_type2;
// need to convert both quantities to a common unit
// which this does, and if necessary promotes value_type
typedef typename quan::meta::binary_op<
arg_type1, quan::meta::minus, arg_type2
>::type finest_grained_type;
finest_grained_type ty = y;
finest_grained_type tx = x;
// 'type' of result of division must be a numeric
typedef typename quan::meta::binary_op<
finest_grained_type,
quan::meta::divides,
finest_grained_type
>::type result_value_type;
// To show this ... check it is a numeric
static_assert(quan::meta::is_numeric<
result_value_type
>::value," result not numeric");
typedef typename quan::angle_<result_value_type>::rad result_type;
#ifndef QUAN_AVR_NO_CPP_STDLIB
using std::atan2;
#else
using ::atan2;
#endif
return result_type {atan2(ty.numeric_value(), tx.numeric_value())};
}
/** 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]);
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 100);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 100);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 100);
}
void myouse::MyouseListener::onOrientationData(myo::Myo * myo, uint64_t timestamp,
const myo::Quaternion<float> & rotation)
{
using std::atan2;
using std::asin;
using std::sin;
double newRoll = atan2(2.0f * (rotation.w() * rotation.x() + rotation.y() * rotation.z()),
1.0f - 2.0f * (rotation.x() * rotation.x() + rotation.y() * rotation.y()));
double newPitch = asin(2.0f * (rotation.w() * rotation.y() - rotation.z() * rotation.x()));
double newYaw = atan2(2.0f * (rotation.w() * rotation.z() + rotation.x() * rotation.y()),
1.0f - 2.0f * (rotation.y() * rotation.y() + rotation.z() * rotation.z()));
double roll = newRoll - rollOffset;
double pitch = newPitch - pitchOffset;
double yaw = newYaw - yawOffset;
if (xDir == myo::xDirectionTowardElbow) pitch *= -1;
if (isScrolling)
{
scroll(-pitch * SCROLL_SPEED);
}
else
{
int x = SCREEN_WIDTH * (0.5 - X_SPEED * yaw);
int y = SCREEN_HEIGHT * (0.5 + Y_SPEED * pitch);
bool dragging = leftDown || rightDown || middleDown;
float dist = sqrt((x - lastX) * (x - lastX) + (y - lastY) * (y - lastY));
if (!dragging || dist > DRAG_THRESHOLD)
{
moveMouse(x, y);
lastX = x;
lastY = y;
}
}
rawRoll = newRoll;
rawPitch = newPitch;
rawYaw = newYaw;
}
inline
quan::angle::rad atan2(QUAN_FLOAT_TYPE const & lhs, QUAN_FLOAT_TYPE const & rhs)
{
#ifndef QUAN_AVR_NO_CPP_STDLIB
using std::atan2;
#else
using ::atan2;
#endif
return quan::angle::rad {atan2(lhs, rhs)};
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(Myo* myo, uint64_t timestamp, const Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 18.
// roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
// pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
// yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void Filter::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
quatX = quat.x(); //Return the x-component of this quaternion's vector
quatY = quat.y(); //Return the y -component of this quaternion's vector
quatZ = quat.z(); //Return the z-component of this quaternion's vector
quatW = quat.w(); //Return the w-component (scalar) of this quaternion's vector
using std::atan2;
using std::asin;
using std::sqrt;
using std::cos;
using std::sin;
using std::max;
using std::min;
roll = -1 * atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
pitch = -1 * atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
yaw = -1 * asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
}
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
MyoFrameOrientationData & orient_data = m_orientation_data;
orient_data.timestamp = timestamp;
orient_data.quaternion = ofQuaternion( quat.x(), quat.y(), quat.z(), quat.w() );
orient_data.euler_roll = roll;
orient_data.euler_pitch = pitch;
orient_data.euler_yaw = yaw;
}
//uses some trigonometry magic to update the position of the current point based on a limited speed given by SPEED_OF_MOVEMENT
Point speedGovernor(Point current, Point destination, double dist) {
double x_dist = destination.x - current.x;
double y_dist = destination.y - current.y;
double distance = sqrt(pow((x_dist), 2) + pow((y_dist), 2));
double angle = atan2(y_dist, x_dist);
if (distance > dist) {
distance = dist;
x_dist = distance*(cos(angle));
y_dist = distance*(sin(angle));
}
current.x = current.x + x_dist;
current.y = current.y + y_dist;
return current;
}
void Rotation::inverseTransform(double& lat, double& lon) const {
const double u = toRadians(lon);
const double v = toRadians(lat);
const double w = cos(v);
const double x = cos(u) * w;
const double y = sin(u) * w;
const double z = sin(v);
const double x2 = a11 * x + a21 * y + a31 * z;
const double y2 = a12 * x + a22 * y + a32 * z;
const double z2 = a13 * x + a23 * y + a33 * z;
lat = toDegrees(asin(z2));
lon = toDegrees(atan2(y2, x2));
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented as a unit quaternion.
// This function takes the position values from the quaternion and assign new values to roll_w, pitch_w and yaw_w every time new data are provided by the Myo.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
// Convert the floating point angles in radians to a clockwise scale from 1 to 127 (good for PureData) with the zero angle in π.
// Need to change the format of the angle? DO IT HERE!
roll_tmp = static_cast<int>( ( roll * 180/M_PI) ); // the value of 200 ensures a positive value for the angle.
}
double great_circle_distance::operator() (coord2d const& pt0,
coord2d const& pt1) const
{
double lon0 = pt0.x * deg2rad;
double lat0 = pt0.y * deg2rad;
double lon1 = pt1.x * deg2rad;
double lat1 = pt1.y * deg2rad;
double dlat = lat1 - lat0;
double dlon = lon1 - lon0;
double sin_dlat = sin(0.5 * dlat);
double sin_dlon = sin(0.5 * dlon);
double a = pow(sin_dlat,2.0) + cos(lat0)*cos(lat1)*pow(sin_dlon,2.0);
double c = 2 * atan2(sqrt(a),sqrt(1 - a));
return R * c;
}
/*
* Compute the initial bearing, **in degrees**, from a starting point to a
* destination point given as lat/lons in radians.
*/
float
initial_bearing_deg(const double lat_a, const double lon_a,
const double lat_b, const double lon_b)
{
using std::sin;
using std::cos;
using std::atan2;
using std::fmod;
const float delta_lon = lon_b - lon_a;
const float opposite = sin(delta_lon) * cos(lat_b);
const float adjacent =
cos(lat_a) * sin(lat_b) - sin(lat_a) * cos(lat_b) * cos(delta_lon);
const float bearing = atan2(opposite, adjacent);
const float bearing_deg = (bearing >= 0.0 ? to_deg(bearing)
: to_deg(bearing) + 360.0);
return bearing_deg;
}
TEST(AgradFwdAtan2,Fvar) {
using stan::math::fvar;
using std::atan2;
fvar<double> x(0.5,1.0);
fvar<double> y(2.3,2.0);
double w = 2.1;
fvar<double> a = atan2(x, y);
EXPECT_FLOAT_EQ(atan2(0.5, 2.3), a.val_);
EXPECT_FLOAT_EQ((1.0 * 2.3 - 0.5 * 2.0) / (0.5 * 0.5 + 2.3 * 2.3), a.d_);
fvar<double> b = atan2(w, x);
EXPECT_FLOAT_EQ(atan2(2.1, 0.5), b.val_);
EXPECT_FLOAT_EQ((-2.1 * 1.0) / (2.1 * 2.1 + 0.5 * 0.5), b.d_);
fvar<double> c = atan2(x, w);
EXPECT_FLOAT_EQ(atan2(0.5, 2.1), c.val_);
EXPECT_FLOAT_EQ((1.0 * 2.1) / (0.5 * 0.5 + 2.1 * 2.1), c.d_);
}
TEST(AgradFvar, atan2) {
using stan::agrad::fvar;
using std::atan2;
fvar<double> x(0.5);
fvar<double> y(2.3);
x.d_ = 1.0;
y.d_ = 2.0;
double w = 2.1;
fvar<double> a = atan2(x, y);
EXPECT_FLOAT_EQ(atan2(0.5, 2.3), a.val_);
EXPECT_FLOAT_EQ((1.0 * 2.3 - 0.5 * 2.0) / (0.5 * 0.5 + 2.3 * 2.3), a.d_);
fvar<double> b = atan2(w, x);
EXPECT_FLOAT_EQ(atan2(2.1, 0.5), b.val_);
EXPECT_FLOAT_EQ((-2.1 * 1.0) / (2.1 * 2.1 + 0.5 * 0.5), b.d_);
fvar<double> c = atan2(x, w);
EXPECT_FLOAT_EQ(atan2(0.5, 2.1), c.val_);
EXPECT_FLOAT_EQ((1.0 * 2.1) / (0.5 * 0.5 + 2.1 * 2.1), c.d_);
}
void WifiDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) {
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
ClientInfo* clientinfo = new ClientInfo();
clientinfo->ID = 0;
clientinfo->message = new std::string("message");
clientinfo->name = "Client";
// Calculate Euler angles (roll and pitch) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
//Transforms the radians to degrees angles.
roll_w = (int)(roll * 180 / M_PI);
pitch_w = (int)(pitch * 180 / M_PI);
//To use in the robot servo motor, roll_w mut be between 45 and 135 degrees
//Gives an interval of 30 degrees to stabilize at 0 degrees.
//The 0 degree on the servo motor is at 90 degrees.
if (roll_w >= -15 && roll_w <= 15) {
roll_w = 90;
}
else if (roll_w < -15) {
//roll_w min value is 45.
if (roll_w < -60)
roll_w = 45;
else
roll_w += 105;
}
else if (roll_w > 15) {
//roll_w max value is 135
if (roll_w > 60)
roll_w = 135;
else
roll_w += 75;
}
//The robot has 6 diffirent speeds
//Gives an interval of 30 degrees to stabilize at 0 degrees.
// Parking state
if (pitch_w >= -15 && pitch_w <= 15) {
pitch_w = 0;
}
//Reverse speeds
else if (pitch_w < -15) {
if (pitch_w >= -35) pitch_w = 4;
else if (pitch_w >= -55) pitch_w = 5;
else pitch_w = 6;
}
//Foward speeds
else if (pitch_w > 15) {
if (pitch_w <= 35) pitch_w = 1;
else if (pitch_w <= 55) pitch_w = 2;
else pitch_w = 3;
}
clientinfo->servo = roll_w;
clientinfo->speed = pitch_w;
client->sendToServer(clientinfo);
delete clientinfo;
}
/**
* @return \e azi0 the azimuth (degrees) of the geodesic line as it crosses
* the equator in a northward direction.
**********************************************************************/
Math::real EquatorialAzimuth() const {
using std::atan2;
return Init() ?
atan2(_salp0, _calp0) / Math::degree() : Math::NaN();
}
/**
* @return \e a1 the arc length (degrees) between the northward equatorial
* crossing and point 1.
*
* The result lies in (−180°, 180°].
**********************************************************************/
Math::real EquatorialArc() const {
using std::atan2;
return Init() ? atan2(_ssig1, _csig1) / Math::degree() : Math::NaN();
}
inline
typename boost::math::tools::promote_args<T0,T1>::type
operator()(const T0 arg1,
const T1 arg2) const {
return atan2(arg1,arg2);
}
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
if (!systemInited) {
systemInitCount++;
if (systemInitCount >= systemDelay) {
systemInited = true;
}
return;
}
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
int cloudSize = laserCloudIn.points.size();
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++) {
point.x = laserCloudIn.points[i].y;
point.y = laserCloudIn.points[i].z;
point.z = laserCloudIn.points[i].x;
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
if (roundedAngle > 0){
scanID = roundedAngle;
}
else {
scanID = roundedAngle + (N_SCANS - 1);
}
if (scanID > (N_SCANS - 1) || scanID < 0 ){
count--;
continue;
}
float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < startOri - M_PI / 2) {
ori += 2 * M_PI;
} else if (ori > startOri + M_PI * 3 / 2) {
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI) {
halfPassed = true;
}
} else {
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2) {
ori += 2 * M_PI;
} else if (ori > endOri + M_PI / 2) {
ori -= 2 * M_PI;
}
}
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else {
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
if (i == 0) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
} else {
ShiftToStartIMU(pointTime);
VeloToStartIMU();
TransformToStartIMU(&point);
}
}
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {
*laserCloud += laserCloudScans[i];
}
int scanCount = -1;
for (int i = 5; i < cloudSize - 5; i++) {
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;
scanEndInd[scanCount - 1] = i - 5;
}
}
}
scanStartInd[0] = 5;
scanEndInd.back() = cloudSize - 5;
for (int i = 5; i < cloudSize - 6; i++) {
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.1) {
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
if (depth1 > depth2) {
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
cloudNeighborPicked[i] = 1;
}
}
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
for (int i = 0; i < N_SCANS; i++) {
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++) {
int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6;
int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1;
for (int k = sp + 1; k <= ep; k++) {
for (int l = k; l >= sp + 1; l--) {
if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
int temp = cloudSortInd[l - 1];
cloudSortInd[l - 1] = cloudSortInd[l];
cloudSortInd[l] = temp;
}
}
}
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1) {
largestPickedNum++;
if (largestPickedNum <= 2) {
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1) {
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);
imuTrans.points[0].x = imuPitchStart;
imuTrans.points[0].y = imuYawStart;
imuTrans.points[0].z = imuRollStart;
imuTrans.points[1].x = imuPitchCur;
imuTrans.points[1].y = imuYawCur;
imuTrans.points[1].z = imuRollCur;
imuTrans.points[2].x = imuShiftFromStartXCur;
imuTrans.points[2].y = imuShiftFromStartYCur;
imuTrans.points[2].z = imuShiftFromStartZCur;
imuTrans.points[3].x = imuVeloFromStartXCur;
imuTrans.points[3].y = imuVeloFromStartYCur;
imuTrans.points[3].z = imuVeloFromStartZCur;
sensor_msgs::PointCloud2 imuTransMsg;
pcl::toROSMsg(imuTrans, imuTransMsg);
imuTransMsg.header.stamp = laserCloudMsg->header.stamp;
imuTransMsg.header.frame_id = "/camera";
pubImuTrans.publish(imuTransMsg);
}