MRPT  2.0.3
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /**
11  * slerp_demo
12  * Execute a Spherical Linear Interpolation given 2 poses.
13  */
14 
16 #include <mrpt/img/TColor.h>
17 #include <mrpt/math/slerp.h>
20 #include <mrpt/system/CTicTac.h>
21 #include <iostream>
22 
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::math;
26 using namespace mrpt::gui;
27 using namespace mrpt::opengl;
28 using namespace mrpt::poses;
29 using namespace mrpt::img;
30 
31 // ------------------------------------------------------
32 // TestSLERP
33 // ------------------------------------------------------
34 void TestSLERP()
35 {
36  CDisplayWindow3D win("Example of SLERP animation", 640, 480);
37 
38  COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
39 
40  win.setCameraAzimuthDeg(-50);
41  win.setCameraElevationDeg(40);
42  win.setCameraZoom(19);
43  win.setCameraPointingToPoint(2, 2, 0);
44 
45  // Modify the scene:
46  // ------------------------------------------------------
47  {
48  auto obj = opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1);
49  obj->setColor(0.4f, 0.4f, 0.4f);
50  theScene->insert(obj);
51  }
52 
53  // Initialize the start, end pose of the animation
54  const TPose3D pose_a(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
55  const TPose3D pose_b(3, 4, 1, 120.0_deg, 40.0_deg, 50.0_deg);
56 
57  {
58  // XYZ corner at A:
61  obj->setPose(pose_a);
62  theScene->insert(obj);
63  }
64  {
65  // XYZ corner at B:
68  obj->setPose(pose_b);
69  theScene->insert(obj);
70  }
71  {
72  // SLERP animated corner:
75  obj->setName("slerp_obj");
76  obj->setPose(pose_a);
77  theScene->insert(obj);
78  }
79 
80  // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
81  win.unlockAccess3DScene();
82 
83  cout << "\n Close the window to exit.\n";
84 
86  static const double MOVE_PERIOD = 1.0;
87  static const double MOVE_PERIOD2 = 2 * MOVE_PERIOD;
88 
89  while (win.isOpen())
90  {
91  // Compute the time:
92  double t = ::fmod(tic.Tac(), MOVE_PERIOD2);
93  if (t < MOVE_PERIOD)
94  t /= MOVE_PERIOD;
95  else
96  t = 1 - (t - MOVE_PERIOD) / MOVE_PERIOD;
97 
98  // SLERP & LERP interpolation:
99  TPose3D pose_interp;
100  mrpt::math::slerp(pose_a, pose_b, t, pose_interp);
101 
102  // Move the scene:
103  COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
104 
105  opengl::CRenderizable::Ptr obj1 = theScene->getByName("slerp_obj");
106  obj1->setPose(pose_interp);
107 
108  // Show text:
109  win.addTextMessage(5, 5, format("t=%.03f", t), 0 /*id*/);
110 
111  // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
112  win.unlockAccess3DScene();
113 
114  // Update window:
115  win.forceRepaint();
116  std::this_thread::sleep_for(5ms);
117  };
118 }
119 
120 // ------------------------------------------------------
121 // MAIN
122 // ------------------------------------------------------
123 int main()
124 {
125  try
126  {
127  TestSLERP();
128  return 0;
129  }
130  catch (const std::exception& e)
131  {
132  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
133  return -1;
134  }
135  catch (...)
136  {
137  printf("Untyped exception!!");
138  return -1;
139  }
140 }
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:50
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
stock_objects.h
mrpt::opengl::stock_objects::CornerXYZSimple
CSetOfObjects::Ptr CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
Definition: StockObjects.cpp:343
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
CDisplayWindow3D.h
mrpt::math::slerp
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:32
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::img
Definition: CCanvas.h:16
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
slerp.h
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
CTicTac.h
TestSLERP
void TestSLERP()
Definition: vision_stereo_rectify/test.cpp:34
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
CGridPlaneXY.h
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
TColor.h
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020