C++ Quaternion::coeffs方法代码示例

  bool testRawDataAcces() {
    bool passed = true;
    Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
    Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
    SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
                       raw, Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
                      raw.data());
    Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
                      map_of_const_rxso3.quaternion().coeffs().eval());

    Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
    Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
    Eigen::Quaternion<Scalar> quat;
    quat.coeffs() = raw2;
    map_of_rxso3.setQuaternion(quat);
    SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
                    quat.coeffs().data());
    Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
    SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
                      map_of_rxso3.quaternion().coeffs().eval());

    RxSO3Type const const_so3(quat);
    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
    }

    RxSO3Type so3(quat);
    for (int i = 0; i < 4; ++i) {
      so3.data()[i] = raw[i];
    }

    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
    }

    for (int i = 0; i < 10; ++i) {
      Matrix3<Scalar> M = Matrix3<Scalar>::Random();
      for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
        Matrix3<Scalar> R = makeRotationMatrix(M);
        Matrix3<Scalar> sR = scale * R;
        SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
                    "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
        Matrix3<Scalar> sR_cols_swapped;
        sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
        SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
                    "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
      }
    }
    return passed;
  }

 // returns the local R,t in nd0 that produces nd1
 // NOTE: returns a postfix rotation; should return a prefix
 void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                   Eigen::Quaternion<double> &qr,
                   Node &nd0, Node &nd1)
 {
   Matrix<double,3,4> tfm;
   Quaterniond q0,q1;
   q0 = nd0.qrot;
   transformW2F(tfm,nd0.trans,q0);
   trans.head(3) = tfm*nd1.trans;
   trans(3) = 1.0;
   q1 = nd1.qrot;
   qr = q0.inverse()*q1;
   qr.normalize();
   if (qr.w() < 0)
     qr.coeffs() = -qr.coeffs();
 }

IGL_INLINE bool igl::snap_to_canonical_view_quat(
  const Eigen::Quaternion<Scalarq> & q,
  const double threshold,
  Eigen::Quaternion<Scalars> & s)
{
  return snap_to_canonical_view_quat<Scalars>(
    q.coeffs().data(),threshold,s.coeffs().data());
}

void SetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
    Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
    		(simFloat)_twistCommands.twist.linear.y,
    		(simFloat)_twistCommands.twist.linear.z);
    Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
    		(simFloat)_twistCommands.twist.angular.y,
    		(simFloat)_twistCommands.twist.angular.z);

    // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
    if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
        linVelocity = orientation*linVelocity;
	    angVelocity = orientation*angVelocity;
    } else {
        std::stringstream ss;
        ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
        ConsoleHandler::printInConsole(ss);
    }

    simResetDynamicObject(_associatedObjectID);

	//Set object velocity
    if (_isStatic){
    	Eigen::Matrix<simFloat, 3, 1> position;
    	simGetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat timeStep = simGetSimulationTimeStep();
    	position = position + timeStep*linVelocity;
    	simSetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat angle = timeStep*angVelocity.norm();
    	if(angle > 1e-6){
    		orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
    	}
    	simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
    } else {
		// Apply the linear velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
		// Apply the angular velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
    }

}

void GetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    ros::Time now = ros::Time::now();


    const simFloat currentSimulationTime = simGetSimulationTime();

    if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){

    	Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
    	Eigen::Matrix<simFloat, 3, 1> linVelocity;
    	Eigen::Matrix<simFloat, 3, 1> angVelocity;
    	bool error = false;

    	// Get object velocity. If the object is not static simGetVelocity is more accurate.
    	if (_isStatic){
    		error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	} else {
    		error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	}

    	// Get object orientation
    	error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;

    	if(!error){

    		linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
			angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame

    		// Fill the status msg
			geometry_msgs::TwistStamped msg;

			msg.twist.linear.x = linVelocity[0];
			msg.twist.linear.y = linVelocity[1];
			msg.twist.linear.z = linVelocity[2];
			msg.twist.angular.x = angVelocity[0];
			msg.twist.angular.y = angVelocity[1];
			msg.twist.angular.z = angVelocity[2];

			msg.header.stamp = now;
			_pub.publish(msg);
			_lastPublishedObjTwistTime = currentSimulationTime;

    	} else {
    	    std::stringstream ss;
    	    ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
    	    ConsoleHandler::printInConsole(ss);
    	}

    }

}

/*
 * This small example show the conversion between Eigen types and KDL types
 *
 * The toKDLxxx() function specify the KDL type in the name because it is impossible
 * to determine the KDL type at compile time when using a dynamic eigen matrix.
 */
int main(int argc, char* argv[]) {
	using namespace KDL;
	using namespace std;
    cout << "Demonstration of simple conversion routines between KDL and Eigen types \n";
    Vector a(1.0,2.0,3.0);
    cout << "KDL Vector " << a << "\n";
    Eigen::Vector3d a_eigen;
    a_eigen = toEigen(a);
    cout << "Eigen representation " << a_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLVector(a_eigen) << "\n";
    cout << endl;

    Twist t(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
    cout << "KDL Vector " << t << "\n";
    Eigen::Matrix<double,6,1> t_eigen;
    t_eigen = toEigen(t);
    cout << "Eigen representation " << t_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLTwist(t_eigen) << "\n";
    cout << endl;


    Wrench w(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
    cout << "KDL Wrench " << t << "\n";
    Eigen::Matrix<double,6,1> w_eigen;
    w_eigen = toEigen(w);
    cout << "Eigen representation " << w_eigen.transpose() << "\n";
    cout << "converted back to KDL " << toKDLTwist(w_eigen) << "\n";

    Rotation R( Rotation::EulerZYX(30*deg2rad,45*deg2rad,0*deg2rad) );
    cout << "KDL Rotation " << R << "\n";
    Eigen::Matrix<double,3,3> R_eigen;
    R_eigen = toEigen( R );
    cout << "Eigen 3x3 matrix " << R_eigen << "\n";
    cout << "converted back to KDL " << toKDLRotation( R_eigen ) << "\n";
    Eigen::Quaternion<double> q;
    q = toEigenQuaternion( R );
    cout << "Eigen Quaternion " << q.coeffs() << "\n";
    cout << "converted back to KDL " << toKDLRotation( q ) << "\n"; 
    cout << endl;
	return 0;
}

  bool testRawDataAcces() {
    bool passed = true;
    Eigen::Matrix<Scalar, 7, 1> raw;
    raw << 0, 1, 0, 0, 1, 3, 2;
    Eigen::Map<Sim3Type const> map_of_const_sim3(raw.data());
    SOPHUS_TEST_APPROX(passed, map_of_const_sim3.quaternion().coeffs().eval(),
                       raw.template head<4>().eval(),
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_APPROX(passed, map_of_const_sim3.translation().eval(),
                       raw.template tail<3>().eval(),
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.translation().data(),
                      raw.data() + 4);
    Eigen::Map<Sim3Type const> const_shallow_copy = map_of_const_sim3;
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
                      map_of_const_sim3.quaternion().coeffs().eval());
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
                      map_of_const_sim3.translation().eval());

    Eigen::Matrix<Scalar, 7, 1> raw2;
    raw2 << 1, 0, 0, 0, 3, 2, 1;
    Eigen::Map<Sim3Type> map_of_sim3(raw.data());
    Eigen::Quaternion<Scalar> quat;
    quat.coeffs() = raw2.template head<4>();
    map_of_sim3.setQuaternion(quat);
    map_of_sim3.translation() = raw2.template tail<3>();
    SOPHUS_TEST_APPROX(passed, map_of_sim3.quaternion().coeffs().eval(),
                       raw2.template head<4>().eval(),
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_APPROX(passed, map_of_sim3.translation().eval(),
                       raw2.template tail<3>().eval(),
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_sim3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_EQUAL(passed, map_of_sim3.translation().data(), raw.data() + 4);
    SOPHUS_TEST_NEQ(passed, map_of_sim3.quaternion().coeffs().data(),
                    quat.coeffs().data());
    Eigen::Map<Sim3Type> shallow_copy = map_of_sim3;
    SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
                      map_of_sim3.quaternion().coeffs().eval());
    SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
                      map_of_sim3.translation().eval());
    Eigen::Map<Sim3Type> const const_map_of_sim3 = map_of_sim3;
    SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.quaternion().coeffs().eval(),
                      map_of_sim3.quaternion().coeffs().eval());
    SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(),
                      map_of_sim3.translation().eval());

    Sim3Type const const_sim3(quat, raw2.template tail<3>().eval());
    for (int i = 0; i < 7; ++i) {
      SOPHUS_TEST_EQUAL(passed, const_sim3.data()[i], raw2.data()[i]);
    }

    Sim3Type se3(quat, raw2.template tail<3>().eval());
    for (int i = 0; i < 7; ++i) {
      SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]);
    }

    for (int i = 0; i < 7; ++i) {
      SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]);
    }
    return passed;
  }

  void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;

    // first get the second frame in first frame coords
    Eigen::Matrix<double,3,1> pc = nr.w2n * t1;

    // Jacobians wrt first frame parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);


    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Eigen::Matrix<double,3,1> pwt;
    pwt = (t1-tr).head(3);   // transform translations

    // dx
    Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,3) = dp;
    // dy
    dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,4) = dp;
    // dz
    dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J0.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = qpmean * s0' * q0' * q1

    // dqdx
    Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
    qr1.coeffs() = q1.coeffs();
    qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());  // qpmean * ds0'/dx
    qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qr0 = qr0*qr1;              // rotate to zero mean
    qrd = qpmean*qr0;           // for normalization check
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,5) = qrn.vec();

    // transpose
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<3,3>(3,0).setZero();


    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*s1*q1

    Eigen::Quaternion<double> qrc;
//.........这里部分代码省略.........