text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          http://www.mrpt.org/                          |
     |                                                                        |
     | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
     | See: http://www.mrpt.org/Authors - All rights reserved.                |
     | Released under BSD License. See details in http://www.mrpt.org/License |
     +------------------------------------------------------------------------+ */
  
  #include "obs-precomp.h"  // Precompiled headers
  
  #include <mrpt/random.h>
  #include <mrpt/serialization/CArchive.h>
  #include <mrpt/obs/CActionRobotMovement3D.h>
  #include <mrpt/poses/CPose3DPDFParticles.h>
  
  using namespace mrpt;
  using namespace mrpt::obs;
  using namespace mrpt::poses;
  using namespace mrpt::random;
  
  IMPLEMENTS_SERIALIZABLE(CActionRobotMovement3D, CAction, mrpt::obs)
  
  /*---------------------------------------------------------------
  						Constructor
    ---------------------------------------------------------------*/
  CActionRobotMovement3D::CActionRobotMovement3D()
  	: poseChange(),
  	  rawOdometryIncrementReading(),
  
  	  motionModelConfiguration(),
  	  hasVelocities(6, false),
  	  velocities(6)
  {
  	velocities.assign(.0);
  }
  
  uint8_t CActionRobotMovement3D::serializeGetVersion() const { return 1; }
  void CActionRobotMovement3D::serializeTo(
  	mrpt::serialization::CArchive& out) const
  {
  	out.WriteAs<uint32_t>(estimationMethod);
  	out << poseChange;
  	out << hasVelocities << velocities;
  	out << timestamp;
  }
  
  void CActionRobotMovement3D::serializeFrom(
  	mrpt::serialization::CArchive& in, uint8_t version)
  {
  	switch (version)
  	{
  		case 0:
  		case 1:
  		{
  			uint32_t i;
  
  			// Read the estimation method first:
  			in >> i;
  			estimationMethod = static_cast<TEstimationMethod>(i);
  
  			in >> poseChange;
  			in >> hasVelocities >> velocities;
  
  			if (version >= 1)
  				in >> timestamp;
  			else
  				timestamp = INVALID_TIMESTAMP;
  		}
  		break;
  		default:
  			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  	};
  }
  
  void CActionRobotMovement3D::computeFromOdometry(
  	const CPose3D& odometryIncrement, const TMotionModelOptions& options)
  {
  	// Set the members data:
  	estimationMethod = emOdometry;
  	rawOdometryIncrementReading = odometryIncrement;
  	motionModelConfiguration = options;
  
  	if (options.modelSelection == mm6DOF)
  		computeFromOdometry_model6DOF(odometryIncrement, options);
  }
  
  /*---------------------------------------------------------------
  						TMotionModelOptions
   ---------------------------------------------------------------*/
  CActionRobotMovement3D::TMotionModelOptions::TMotionModelOptions()
  	: mm6DOFModel()
  {
  	mm6DOFModel.nParticlesCount = 300;
  	mm6DOFModel.a1 = 0.05f;
  	mm6DOFModel.a2 = 0.05f;
  	mm6DOFModel.a3 = 0.05f;
  	mm6DOFModel.a4 = 0.05f;
  	mm6DOFModel.a5 = 0.05f;
  	mm6DOFModel.a6 = 0.05f;
  	mm6DOFModel.a7 = 0.05f;
  	mm6DOFModel.a8 = 0.05f;
  	mm6DOFModel.a9 = 0.05f;
  	mm6DOFModel.a10 = 0.05f;
  	mm6DOFModel.additional_std_XYZ = 0.001f;
  	mm6DOFModel.additional_std_angle = DEG2RAD(0.05f);
  }
  
  /*---------------------------------------------------------------
  				computeFromOdometry_model6DOF
  	---------------------------------------------------------------*/
  void CActionRobotMovement3D::computeFromOdometry_model6DOF(
  	const CPose3D& odometryIncrement, const TMotionModelOptions& o)
  {
  	// The Gaussian PDF:
  	// ---------------------------
  	CPose3DPDFParticles* aux;
  	const mrpt::math::TPose3D nullPose(0, 0, 0, 0, 0, 0);
  
  	mrpt::poses::CPose3DPDF::Ptr poseChangeTemp =
  		mrpt::make_aligned_shared<CPose3DPDFParticles>();
  	aux = dynamic_cast<CPose3DPDFParticles*>(poseChangeTemp.get());
  
  	// Set the number of particles:
  	aux->resetDeterministic(nullPose, o.mm6DOFModel.nParticlesCount);
  
  	// The motion model: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci,
  	// F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF
  	// Monte Carlo Localization 4th Workshop on Planning, Perception and
  	// Navigation for Intelligent Vehicles, IROS, 2012
  
  	/*
  		The brief description:
  		Find XYZ deltas: dX=x(t)-x(t-1); dY=y(t)-y(t-1); dZ=z(t)-z(t-1)
  		Find angular deltas (normalised): dRoll=roll(t)-roll(t-1);
  	   dPitch=pitch(t)-pitch(t-1); dYaw=yaw(t)-yaw(t-1)
  
  		Here t - is time step
  
  		 Calculate the odometry for 6DOF:
  
  
  				yaw1 = atan2 (dY,dX)
  				pitch1 = atan2 (dZ , sqrt(dX^2 + dY^2 + dZ^2) )
  				trans = sqrt(dX^2 + dY^2 + dZ^2)
  				roll = dRoll
  				yaw2= dYaw
  				pitch2= dPitch
  
  
  		Model the error:
  
  
  				yaw1_draw = yaw1 + sample(a1 * yaw1 + a2 * trans )
  				pitch1_draw = pitch1 + sample(a3 * dZ )
  				trans_draw = trans + sample( a4 * trans + a5 * yaw2 + a6 * (roll
  	   + pitch2 ) )
  				roll_draw = roll + sample(a7 * roll)
  				pitch2_draw = pitch2 + sample(a8 * pitch2 )
  				yaw2_draw = yaw2 + sample(a9 * yaw2 + a10 * trans )
  
  		 Here sample() - sampling from zero mean Gaussian distribution
  
  		Calculate the spherical coordinates:
  				x = trans_draw * sin( pitch1_draw ) * cos ( yaw1_draw )
  				y = trans_draw * sin( pitch1_draw ) * sin ( yaw1_draw )
  				z = trans_draw * cos( pitch1_draw )
  		 roll = roll_draw
  		pitch = pitch1_draw + pitch2_draw
  			yaw = yaw1_draw + yaw2_draw
  
  		normalize_angle(roll, pitch, yaw )
  	*/
  
  	// The increments in odometry:
  	float Ayaw1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
  					  ? atan2(odometryIncrement.y(), odometryIncrement.x())
  					  : 0;
  
  	float Atrans = odometryIncrement.norm();
  
  	float Apitch1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0 ||
  					 odometryIncrement.z() != 0)
  						? atan2(odometryIncrement.z(), odometryIncrement.norm())
  						: 0;
  
  	float Aroll = odometryIncrement.roll();
  	float Apitch2 = odometryIncrement.pitch();
  	float Ayaw2 = odometryIncrement.yaw();
  
  	// Draw samples:
  	for (size_t i = 0; i < o.mm6DOFModel.nParticlesCount; i++)
  	{
  		float Ayaw1_draw =
  			Ayaw1 + (o.mm6DOFModel.a1 * Ayaw1 + o.mm6DOFModel.a2 * Atrans) *
  						getRandomGenerator().drawGaussian1D_normalized();
  		float Apitch1_draw =
  			Apitch1 + (o.mm6DOFModel.a3 * odometryIncrement.z()) *
  						  getRandomGenerator().drawGaussian1D_normalized();
  		float Atrans_draw =
  			Atrans + (o.mm6DOFModel.a4 * Atrans + o.mm6DOFModel.a5 * Ayaw2 +
  					  o.mm6DOFModel.a6 * (Aroll + Apitch2)) *
  						 getRandomGenerator().drawGaussian1D_normalized();
  
  		float Aroll_draw =
  			Aroll + (o.mm6DOFModel.a7 * Aroll) *
  						getRandomGenerator().drawGaussian1D_normalized();
  		float Apitch2_draw =
  			Apitch2 + (o.mm6DOFModel.a8 * Apitch2) *
  						  getRandomGenerator().drawGaussian1D_normalized();
  		float Ayaw2_draw =
  			Ayaw2 + (o.mm6DOFModel.a9 * Ayaw2 + o.mm6DOFModel.a10 * Atrans) *
  						getRandomGenerator().drawGaussian1D_normalized();
  
  		// Output:
  		aux->m_particles[i].d.x =
  			Atrans_draw * sin(Apitch1_draw) * cos(Ayaw1_draw) +
  			motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
  				getRandomGenerator().drawGaussian1D_normalized();
  		aux->m_particles[i].d.y =
  			Atrans_draw * sin(Apitch1_draw) * sin(Ayaw1_draw) +
  			motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
  				getRandomGenerator().drawGaussian1D_normalized();
  		aux->m_particles[i].d.z =
  			Atrans_draw * cos(Apitch1_draw) +
  			motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
  				getRandomGenerator().drawGaussian1D_normalized();
  
  		double new_yaw =
  			Ayaw1_draw + Ayaw2_draw +
  			motionModelConfiguration.mm6DOFModel.additional_std_angle *
  				getRandomGenerator().drawGaussian1D_normalized();
  		double new_pitch =
  			Apitch1_draw + Apitch2_draw +
  			motionModelConfiguration.mm6DOFModel.additional_std_angle *
  				getRandomGenerator().drawGaussian1D_normalized();
  		double new_roll =
  			Aroll_draw +
  			motionModelConfiguration.mm6DOFModel.additional_std_angle *
  				getRandomGenerator().drawGaussian1D_normalized();
  
  		aux->m_particles[i].d.yaw = new_yaw;
  		aux->m_particles[i].d.pitch = new_pitch;
  		aux->m_particles[i].d.roll = new_roll;
  	}
  
  	poseChange.copyFrom(*poseChangeTemp);
  }
  