本文整理汇总了C++中DVector3::pitch方法的典型用法代码示例。如果您正苦于以下问题：C++ DVector3::pitch方法的具体用法？C++ DVector3::pitch怎么用？C++ DVector3::pitch使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类DVector3的用法示例。
在下文中一共展示了DVector3::pitch方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: qc_odometry_odometry_message_handler
void qc_odometry_odometry_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){
(void) clientData; //no warning
IPC_RETURN_TYPE err = IPC_OK;
prev_omsg = omsg;
err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &omsg, sizeof(omsg));
if (newImuMessageAvailable){
newImuMessageAvailable = false;
prev_vmsg = vmsg;
vmsg.timestamp_sec = omsg.timestamp_sec;
vmsg.timestamp_usec = omsg.timestamp_usec;
if (msgcounter > 2)
validData = true;
if (validData){
double dtv = omsg.timestamp_sec - prev_omsg.timestamp_sec + 1e-6 * (omsg.timestamp_usec - prev_omsg.timestamp_usec);
double dta = vmsg.timestamp_sec - prev_vmsg.timestamp_sec + 1e-6 * (vmsg.timestamp_usec - prev_vmsg.timestamp_usec);
vmsg.x = omsg.x;
vmsg.y = omsg.y;
vmsg.z = omsg.z;
vmsg.q0 = omsg.q0;
vmsg.q1 = omsg.q1;
vmsg.q2 = omsg.q2;
vmsg.q3 = omsg.q3;
vmsg.roll = omsg.roll;
vmsg.pitch = omsg.pitch;
vmsg.yaw = omsg.yaw;
Transformation3<double> delta;
Transformation3<double> p0(DVector3(prev_omsg.x, prev_omsg.y, prev_omsg.z), Quaternion<double>(prev_omsg.q0, prev_omsg.q1, prev_omsg.q2, prev_omsg.q3));
Transformation3<double> p1(DVector3(omsg.x, omsg.y, omsg.z), Quaternion<double>(omsg.q0, omsg.q1, omsg.q2, omsg.q3));
delta = p0.inv() * p1;
///velocity && acceleration based on omsg
if (dtv > 1e-7){
dtv = 1./dtv;
vmsg.vx = delta.translationVector.x() * dtv;
vmsg.vy = delta.translationVector.y() * dtv;
vmsg.vz = delta.translationVector.z() * dtv;
if (dta > 1e-7){
///take into account the different orientations of the velocities!
p0.translationVector = DVector3(0,0,0);
p0.rotationQuaternion = Quaternion<double>(prev_vmsg.q0, prev_vmsg.q1, prev_vmsg.q2, prev_vmsg.q3);
p0.translationVector = p0 * DVector3(prev_vmsg.vx, prev_vmsg.vy, prev_vmsg.vz);
p1.translationVector = DVector3(0,0,0);
p1.rotationQuaternion = Quaternion<double>(vmsg.q0, vmsg.q1, vmsg.q2, vmsg.q3);
p1.translationVector = p1 * DVector3(vmsg.vx, vmsg.vy, vmsg.vz);
delta = p0.inv() * p1;
dta = 1./dta;
vmsg.avx = delta.translationVector.x() * dta;
vmsg.avy = delta.translationVector.y() * dta;
vmsg.avz = delta.translationVector.z() * dta;
} else {
vmsg.avx = vmsg.avy = vmsg.avz = 0;
}
} else {
vmsg.avx = vmsg.avy = vmsg.avz = 0;
vmsg.vx = vmsg.vy = vmsg.vz = 0;
}
///acceleration based on imu data
///FIXME
///acceleration based on tan roll and tan pitch
Quaternion<double> q(imsg.q0, imsg.q1, imsg.q2, imsg.q3);
DVector3 an = q.toAngles();
vmsg.axtanp = tan(an.pitch()) * 9.81;
vmsg.aytanr = tan(an.roll()) * 9.81;
qc_odometry_publish_velocity_message(&vmsg);
}
}
IPC_freeByteArray(callData);
IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&omsg);
msgcounter++;
if (!(msgcounter%10))
std::cerr << "." << std::flush;
}