/**
  *  @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);
		}
}
Example #3
0
    // 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;
        }
    }
Example #4
0
    // 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);
    }
Example #5
0
/*!
 * 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;
        }
}
Example #6
0
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_);
}
Example #7
0
// 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));
		}
    }
Example #10
0
 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;
 }
Example #11
0
   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())};
   }
Example #12
0
 /** 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]);
 }
Example #13
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);
}
Example #14
0
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;
}
Example #15
0
 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)};
 }
Example #16
0
		// 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);

		}
Example #17
0
	// 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()))));
	}
Example #18
0
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;
}
Example #20
0
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.
 }
Example #22
0
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; 
}
Example #23
0
/*
 * 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;
}
Example #24
0
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_);
}
Example #25
0
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;


}
Example #27
0
 /**
  * @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();
 }
Example #28
0
 /**
  * @return \e a1 the arc length (degrees) between the northward equatorial
  *   crossing and point 1.
  *
  * The result lies in (&minus;180&deg;, 180&deg;].
  **********************************************************************/
 Math::real EquatorialArc() const {
   using std::atan2;
   return Init() ? atan2(_ssig1, _csig1) / Math::degree() : Math::NaN();
 }
Example #29
0
 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);
}