15 float x = v.
x/2,
y = v.
y/2,
z = v.
z/2, th2, th = sqrtf(th2 = x*x +
y*
y +
z*
z);
16 float c = cosf(th),
s = th2 < sqrtf(120*FLT_EPSILON) ? 1-th2/6 : sinf(th)/th;
24 a.
x * b.
w + a.
w * b.
x - a.
z * b.
y + a.
y * b.
z,
25 a.
y * b.
w + a.
z * b.
x + a.
w * b.
y - a.
x * b.
z,
26 a.
z * b.
w - a.
y * b.
x + a.
x * b.
y + a.
w * b.
z,
27 a.
w * b.
w - a.
x * b.
x - a.
y * b.
y - a.
z * b.
z,
47 int main(
int argc,
char * argv[])
try 62 std::lock_guard<std::mutex>
lock(mutex);
64 rs2_pose pose_data = fp.get_pose_data();
66 double now_ms =
static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(
now).
count());
67 double pose_time_ms = fp.get_timestamp();
68 float dt_s =
static_cast<float>(std::max(0., (now_ms - pose_time_ms)/1000.));
70 std::cout <<
"Predicted " << std::fixed << std::setprecision(3) << dt_s*1000 <<
"ms " <<
82 std::this_thread::sleep_for(std::chrono::milliseconds(100));
92 catch (
const std::exception& e)
static const textual_icon lock
GLboolean GLboolean GLboolean b
rs2_quaternion quaternion_multiply(rs2_quaternion a, rs2_quaternion b)
rs2_vector angular_acceleration
unsigned int tracker_confidence
rs2_vector angular_velocity
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
GLboolean GLboolean GLboolean GLboolean a
Quaternion used to represent rotation.
const std::string & get_failed_args() const
rs2_quaternion quaternion_exp(rs2_vector v)
void enable_stream(rs2_stream stream_type, int stream_index, int width, int height, rs2_format format=RS2_FORMAT_ANY, int framerate=0)
3D vector in Euclidean coordinate space
rs2_pose predict_pose(rs2_pose &pose, float dt_s)
int main(int argc, char *argv[])
const std::string & get_failed_function() const