C++ Quaterniond::conjugate方法代码示例

    inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
                                   BodyProcessModel &processModel,
                                   CannedIMUMeasurement const &meas) {

        Eigen::Vector3d angVel;
        meas.restoreAngVel(angVel);
        Eigen::Vector3d var;
        meas.restoreAngVelVariance(var);
#if 0
        static const double dt = 0.02;
        /// Rotate it into camera space - it's bTb and we want cTc
        /// @todo do this without rotating into camera space?
        Eigen::Quaterniond cTb = state.getQuaternion();
        // Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
        /// Turn it into an incremental quat to do the space transformation
        Eigen::Quaterniond incrementalQuat =
            cTb * angVelVecToIncRot(angVel, dt) * cTb.conjugate();
        /// Then turn it back into an angular velocity vector
        angVel = incRotToAngVelVec(incrementalQuat, dt);
        // angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
        /// @todo transform variance?

        kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
        kalman::correct(state, processModel, kalmanMeas);
#else
        kalman::IMUAngVelMeasurement kalmanMeas{angVel, var};
        auto correctionInProgress =
            kalman::beginUnscentedCorrection(state, kalmanMeas);
        auto outputMeas = [&] {
            std::cout << "state: " << state.angularVelocity().transpose()
                      << " and measurement: " << angVel.transpose();
        };
        if (!correctionInProgress.stateCorrectionFinite) {
            std::cout
                << "Non-finite state correction in applying angular velocity: ";
            outputMeas();
            std::cout << "\n";
            return;
        }
        if (!correctionInProgress.finishCorrection(true)) {
            std::cout << "Non-finite error covariance after applying angular "
                         "velocity: ";
            outputMeas();
            std::cout << "\n";
        }
#endif
    }

    inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
                                   BodyProcessModel &processModel,
                                   CannedIMUMeasurement const &meas) {

        Eigen::Vector3d angVel;
        meas.restoreAngVel(angVel);
        Eigen::Vector3d var;
        meas.restoreAngVelVariance(var);

        /// Rotate it into camera space - it's bTb and we want cTc
        /// @todo do this without rotating into camera space?
        Eigen::Quaterniond cTb = state.getQuaternion();
        //Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
        Eigen::Quaterniond incrementalQuat = cTb * util::quat_exp_map(angVel).exp() *
            cTb.conjugate();
        angVel = util::quat_exp_map(incrementalQuat).ln();
        // angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
        /// @todo transform variance?

        kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
        kalman::correct(state, processModel, kalmanMeas);
    }

void Transformer::transform() {
  // convert translation and rotation from local to global coords
  Eigen::Quaterniond refq = ref_.get_rotation();
  Eigen::Vector3d global_t(refq * t_);
  Eigen::Quaterniond global_q(pick_positive(refq * q_ * refq.conjugate()));
  // get rb centroid as a translation
  Eigen::Vector3d centroid(Referential(m_, target_).get_centroid());
  // transform each rigid member
  RigidBody d(m_, target_);
  ParticleIndexes pis(d.get_member_particle_indexes());
  for (unsigned i = 0; i < pis.size(); i++) {
    XYZ xyz(m_, pis[i]);
    Eigen::Vector3d coords;
    coords << xyz.get_x(), xyz.get_y(), xyz.get_z();
    Eigen::Vector3d newcoords = global_q * (coords - centroid) + centroid
                                    + global_t;
    xyz.set_x(newcoords(0));
    xyz.set_y(newcoords(1));
    xyz.set_z(newcoords(2));
  }
  // update rigid body
  d.set_reference_frame_from_members(pis);
}

void des_thrusters::step(double delta)
{
    bool softkilled;
    shm_get(switches, soft_kill, softkilled);
    shm_getg(settings_control, shm_settings);

    if (softkilled || !shm_settings.enabled) {
        f -= f;
        t -= t;
        return;
    }
    
    // Read PID settings & desires.
    // (would switching to watchers improve performance?)

    SHM_GET_PID(settings_heading, shm_hp, hp)
    SHM_GET_PID(settings_pitch, shm_pp, pp)
    SHM_GET_PID(settings_roll, shm_rp, rp)
    SHM_GET_PID(settings_velx, shm_xp, xp)
    SHM_GET_PID(settings_vely, shm_yp, yp)
    SHM_GET_PID(settings_depth, shm_dp, dp)

    shm_getg(desires, shm_desires);

    // Read current orientation, velocity & position (in the model frame).

    // Orientation quaternion in the model frame.
    Eigen::Quaterniond qm = q * mtob_rq;
    Eigen::Vector3d rph = quat_to_euler(qm);

    // Component of the model quaternion about the z-axis (heading-only rotation).
    Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0);

    // Velocity in heading modified world space.
    Eigen::Vector3d vp = qh.conjugate() * v;
    
    // Step the PID loops.

    Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD,
                                                          shm_desires.pitch * DEG_TO_RAD,
                                                          shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG;
    
    double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG);
    double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG);
    double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG);

    double xo = xp.step(delta, shm_desires.speed, vp[0]);
    double yo = yp.step(delta, shm_desires.sway_speed, vp[1]);
    double zo = dp.step(delta, shm_desires.depth, x[2]);

    // f is in the heading modified world frame.
    // t is in the model frame.
    // We will work with f and b in the model frame until the end of this
    // function.

    f[0] = shm_settings.velx_active ? xo : 0;
    f[1] = shm_settings.vely_active ? yo : 0;
    f[2] = shm_settings.depth_active ? zo : 0;

    f *= m;

    Eigen::Vector3d w_in;
    w_in[0] = shm_settings.roll_active ? ro : 0;
    w_in[1] = shm_settings.pitch_active ? po : 0;
    w_in[2] = shm_settings.heading_active ? ho : 0;

    // TODO Avoid this roundabout conversion from hpr frame
    // to world frame and back to model frame.
    Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0);
    Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) +
                                     qh * Eigen::Vector3d(0, w_in[1], 0) +
                                    qhp * Eigen::Vector3d(w_in[0], 0, 0));

    t = btom_rm * q.conjugate() * I * q * mtob_rm * w;

    // Output diagnostic information. Shown by auv-control-helm.

    SHM_PUT_PID(control_internal_heading, ho)
    SHM_PUT_PID(control_internal_pitch, po)
    SHM_PUT_PID(control_internal_roll, ro)
    SHM_PUT_PID(control_internal_velx, xo)
    SHM_PUT_PID(control_internal_vely, yo)
    SHM_PUT_PID(control_internal_depth, zo)

    // Subtract passive forces.
    // (this implementation does not support discrimination!)
    if (shm_settings.buoyancy_forces || shm_settings.drag_forces) {
        // pff is in the body frame.
        screw ps = pff();
        f -= qh.conjugate() * q * ps.first;
        t -= btom_rm * ps.second;
    }

    // Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of
    // energy the sub can output (e.g. can't move forward at full speed
    // and pitch at full speed at the same time).
    // Doesn't really account for real-world thruster configurations (e.g.
    // it might be possible to move forward at full speed and ascend at
    // full speed at the same time) but it's just an approximation.
    for (int i = 0; i < 3; i++) {
//.........这里部分代码省略.........

bool GazeboInterface::readJointStates()
{
	if(!ROSControlInterface::readJointStates())
		return false;

	if(m_last_modelStates)
	{
		std::vector<std::string>::const_iterator it = std::find(
			m_last_modelStates->name.begin(), m_last_modelStates->name.end(),
			m_modelName
		);

		if(it != m_last_modelStates->name.end())
		{
			int idx = it - m_last_modelStates->name.begin();

			//
			// Orientation feedback
			//

			// Retrieve the robot pose
			const geometry_msgs::Pose& pose = m_last_modelStates->pose[idx];

			// Set the robot orientation
			Eigen::Quaterniond quat;
			tf::quaternionMsgToEigen(pose.orientation, quat);
			quat.normalize();
			m_model->setRobotOrientation(quat);

			// Provide /odom if wanted. Convention: /odom is on the floor.
			if(m_publishOdom && m_last_modelStatesStamp - m_initTime > ros::Duration(3.0))
			{
				tf::StampedTransform trans;
				trans.frame_id_ = "/odom";
				trans.child_frame_id_ = "/ego_rot";
				trans.stamp_ = m_last_modelStatesStamp;
				trans.setIdentity();

				tf::Vector3 translation;
				tf::pointMsgToTF(pose.position, translation);

				trans.setOrigin(translation);

				Eigen::Quaterniond rot;
				rot = Eigen::AngleAxisd(m_model->robotEYaw(), Eigen::Vector3d::UnitZ());

				tf::Quaternion quat;
				tf::quaternionEigenToTF(rot, quat);

				trans.setRotation(quat);

				ROS_DEBUG("robot pos: Z = %f, yaw: %f, stamp: %f", translation.z(), m_model->robotEYaw(), trans.stamp_.toSec());
				ROS_DEBUG("robot pos tf: %f %f %f %f %f %f %f",
					trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z(),
					trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()
				);

				m_pub_tf.sendTransform(trans);
			}

			//
			// Angular velocity feedback
			//

			// Retrieve the robot twist
			const geometry_msgs::Twist& twist = m_last_modelStates->twist[idx];

			// Retrieve the robot angular velocity in global coordinates
			Eigen::Vector3d globalAngularVelocity;
			tf::vectorMsgToEigen(twist.angular, globalAngularVelocity);

			// Set the measured angular velocity (local coordinates)
			m_model->setRobotAngularVelocity(quat.conjugate() * globalAngularVelocity);

			//
			// Acceleration feedback
			//

			// We'd need an extra Gazebo plugin for acceleration sensing.
			// For now we can do without and simply calculate the current
			// acceleration due to gravity (neglect inertial accelerations).
			Eigen::Vector3d globalGravityAcceleration(0.0, 0.0, 9.81);

			// Set the measured acceleration (local coordinates)
			m_model->setAccelerationVector(quat.conjugate() * globalGravityAcceleration);

			//
			// Magnetic field vector feedback
			//

			// We assume that north is positive X. The values of 0.20G (horiz)
			// and 0.44G (vert) are approximately valid for central europe.
			Eigen::Vector3d globalMagneticVector(0.20, 0.00, -0.44); // In gauss

			// Set the measured magnetic field vector (local coordinates)
			m_model->setMagneticFieldVector(quat.conjugate() * globalMagneticVector);
		}
	}

	// Return success
//.........这里部分代码省略.........

void
EstimatorFull::UpdateCamera(const Eigen::Vector3d &p_c_v, const Eigen::Quaterniond &q_c_v,
	const Eigen::Matrix<double,6,6> &R,
	bool absolute, bool isKeyframe, double dLambda )
{
	Eigen::QuaternionAd q_c_kf;
	Eigen::Vector3d p_c_kf;
	if (absolute)
	{
		q_c_kf = Eigen::QuaternionAd(q_c_v.conjugate() * q_kf_v.toQuat().conjugate());
	    p_c_kf = q_kf_v.toQuat().toRotationMatrix() * (p_c_v - p_kf_v);
	}
	else
	{
		q_c_kf.q = q_c_v;
		p_c_kf = p_c_v;
	}

	Matrix3d C_q_w_v = q_w_v.toQuat().toRotationMatrix();
	Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
	Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();

	// Build Jacobian
	Matrix<double,3,28> H_p;
	H_p <<
	C_q_w_v.transpose() * exp(lambda),
	Matrix3d::Zero(),
	-C_q_w_v.transpose()*C_q_i_w.transpose()*crossMat(p_c_i)*exp(lambda),
	Matrix3d::Zero(),
	Matrix3d::Zero(),
	(C_q_w_v.transpose()*(C_q_i_w.transpose()*p_c_i + p_i_w) + p_w_v) * exp(lambda),
	C_q_w_v.transpose()*C_q_i_w.transpose()*exp(lambda),
	Matrix3d::Zero(),
	Matrix3d::Identity()*exp(lambda),
	-C_q_w_v.transpose()*crossMat( p_i_w + C_q_i_w.transpose()*p_c_i )*exp(lambda);

	Matrix<double,3,28> H_q;
	H_q <<
	Matrix3d::Zero(),
	Matrix3d::Zero(),
	C_q_c_i,
	Matrix3d::Zero(),
	Matrix3d::Zero(),
	Vector3d::Zero(),
	Matrix3d::Zero(),
	Matrix3d::Identity(),
	Matrix3d::Zero(),
	C_q_c_i*C_q_i_w;

	Matrix<double,6,28> H;
	H << H_p, H_q;

	// Calculate residual

	Vector3d r_p = p_c_kf - ( C_q_w_v.transpose()*(p_i_w + C_q_i_w.transpose()*p_c_i) + p_w_v ) * exp(lambda);
	Quaterniond r_q = (q_c_kf.toQuat()*( q_c_i.toQuat()*q_i_w.toQuat()*q_w_v.toQuat() ).conjugate()).conjugate();

	Matrix<double,6,1> r;
	r << r_p, 2*r_q.x(), 2*r_q.y(), 2*r_q.z();

	// Calculate kalmn gain
	Matrix<double,6,6> S = H*P*H.transpose() + R;
	//K = P*H.transpose()*S^-1;
	//TODO: use cholsky to solve K*S = P*H.transposed()? 
	// (K*S)' = (P*H')'
	// S'*K' = H*P'
	// K' = S.transpose.ldlt().solve(H*P.transpose)
	Matrix<double,28,6> K = (S.ldlt().solve(H*P)).transpose(); // P and S are symmetric
	//Matrix<double,28,6> K = P*H.transpose()*S.inverse();//S.lu().solve(H*P).transpose();
	
	Matrix<double,28,1>	x_error = K*r;

	// Apply Kalman gain
	ApplyCorrection( x_error );
	Matrix<double,28,28> T = Matrix<double,28,28>::Identity() - K*H;
	P = T*P*T.transpose() + K*R*K.transpose();

	// Symmetrize
	P = (P + P.transpose())/2.0;

	if (isKeyframe)
	{
		// Add new keyframe from current position/camera pose
		UpdateKeyframe( );

		// Add noise to lambda
		P(15,15) += dLambda;

		// Update keyframe reference, if given path is absolute
		if (absolute)
		{
			p_kf_v = p_c_v;
			q_kf_v.q = q_c_v;
		}
	}
}