Interface class for the SLAM
pipeline.
More...
#include <ocl_processing.hpp>
Public Member Functions | |
OCLSLAM (Kinect *kinect, octomap::OcTree &map) | |
Constructor. More... | |
~OCLSLAM () | |
Destructor. More... | |
void | init () |
Initializes the SLAM pipeline. More... | |
void | registerPointCloud () |
Registers a point cloud. More... | |
void | write (std::string filename=std::string("map.ot")) |
Stores an occupancy map on disk. More... | |
void | writeBinary (std::string filename=std::string("map.bt")) |
Stores an binary map on disk. More... | |
void | display () |
Prints on the console results about the current registration and localization. More... | |
bool | getSLAMStatus () |
Gets the status of the automated SLAM process. More... | |
void | setSLAMStatus (bool flag) |
Sets the status of the automated SLAM process. More... | |
void | toggleSLAMStatus () |
Toggles the status of the automated SLAM process. More... | |
bool | getGFRGBStatus () |
Gets the status of the RGB Guided Filter. More... | |
void | setGFRGBStatus (bool flag) |
Sets the status of the RGB Guided Filter. More... | |
void | toggleGFRGBStatus () |
Toggles the status of the RGB Guided Filter. More... | |
bool | getGFDStatus () |
Gets the status of the Depth Guided Filter. More... | |
void | setGFDStatus (bool flag) |
Sets the status of the Depth Guided Filter. More... | |
void | toggleGFDStatus () |
Toggles the status of the Depth Guided Filter. More... | |
int | getRGBNormalization () |
Gets the status of the RGB normalization. More... | |
void | setRGBNormalization (int flag) |
Sets the status of the RGB normalization. More... | |
void | toggleRGBNormalization () |
Toggles the status of the RGB normalization. More... | |
int | getGFRGBRadius () |
Gets the window radius \(r\) for the guided filter performed on the RGB frame. More... | |
void | setGFRGBRadius (int radius) |
Sets the window radius \(r\) for the guided filter performed on the RGB frame. More... | |
float | getGFRGBEps () |
Gets the variability threshold \(\epsilon\) for the guided filter performed on the RGB frame. More... | |
void | setGFRGBEps (float eps) |
Sets the variability threshold \(\epsilon\) for the guided filter performed on the RGB frame. More... | |
int | getGFDRadius () |
Gets the window radius \(r\) for the guided filter performed on the Depth frame. More... | |
void | setGFDRadius (int radius) |
Sets the window radius \(r\) for the guided filter performed on the Depth frame. More... | |
float | getGFDEps () |
Gets the variability threshold \(\epsilon\) for the guided filter performed on the Depth frame. More... | |
void | setGFDEps (float eps) |
Sets the variability threshold \(\epsilon\) for the guided filter performed on the Depth frame. More... | |
float | getGFDScaling () |
Gets the scaling applied to the depth frame for processing with the guided filter. More... | |
void | setGFDScaling (float scaling) |
Sets the scaling applied to the depth frame for processing with the guided filter. More... | |
float | getSensorFocalLength () |
Gets the sensor's focal length. More... | |
void | setSensorFocalLength (float f) |
Sets the sensor's focal length. More... | |
float | getRBCAlpha () |
Gets the parameter \( \alpha \) used in the distance function for the RBC data structure. More... | |
void | setRBCAlpha (float _a) |
Sets the parameter \( \alpha \) used in the distance function for the RBC data structure. More... | |
float | getICPSScaling () |
Gets the scaling applying to the deviations when computing matrix S in the ICP algorithm, for managing floating point arithmetic issues. More... | |
void | setICPSScaling (float _c) |
Sets the scaling applying to the deviations when computing matrix S in the ICP algorithm, for managing floating point arithmetic issues. More... | |
unsigned int | getICPMaxIterations () |
Gets the maximum number of iterations considered for an ICP registration. More... | |
void | setICPMaxIterations (unsigned int maxIter) |
Sets the maximum number of iterations considered for an ICP registration. More... | |
double | getICPAngleThreshold () |
Gets the angle threshold (in degrees) for the convergence check of the ICP. More... | |
void | setICPAngleThreshold (double at) |
Sets the angle threshold (in degrees) for the convergence check of the ICP. More... | |
double | getICPTranslationThreshold () |
Gets the translation threshold (in mm) for the convergence check of the ICP. More... | |
void | setICPTranslationThreshold (double tt) |
Sets the translation threshold (in mm) for the convergence check of the ICP. More... | |
Public Attributes | |
std::function< void()> | slam |
Performs the SLAM process. More... | |
volatile int | timeStep |
Eigen::Matrix3f | R_g |
Eigen::Quaternionf | q_g |
Eigen::Vector3f | t_g |
cl_float | s_g |
Interface class for the SLAM
pipeline.
Retrieves data, registers point clouds, and builds a map.
CR | configures the class with different methods of rotation computation. |
CW | configures the class for performing either regular or weighted computation. |
OCLSLAM< CR, CW >::OCLSLAM | ( | Kinect * | kinect, |
octomap::OcTree & | map | ||
) |
Constructor.
Initializes the OpenCL environment, the OpenGL buffers, and the classes for the SLAM
pipeline.
[in] | kinect | initialized Kinect device. |
[in] | map | OctoMap structure for building the map. |
Destructor.
Prints on the console results about the current registration and localization.
|
inline |
Gets the variability threshold \(\epsilon\) for the guided filter performed on the Depth frame.
|
inline |
Gets the window radius \(r\) for the guided filter performed on the Depth frame.
|
inline |
Gets the scaling applied to the depth frame for processing with the guided filter.
|
inline |
Gets the status of the Depth Guided Filter.
|
inline |
Gets the variability threshold \(\epsilon\) for the guided filter performed on the RGB frame.
|
inline |
Gets the window radius \(r\) for the guided filter performed on the RGB frame.
|
inline |
Gets the status of the RGB Guided Filter.
|
inline |
Gets the angle threshold (in degrees) for the convergence check of the ICP.
|
inline |
Gets the maximum number of iterations considered for an ICP registration.
|
inline |
Gets the scaling applying to the deviations when computing matrix S
in the ICP algorithm, for managing floating point arithmetic issues.
|
inline |
Gets the translation threshold (in mm) for the convergence check of the ICP.
|
inline |
Gets the parameter \( \alpha \) used in the distance function for the RBC data structure.
|
inline |
Gets the status of the RGB normalization.
|
inline |
Gets the sensor's focal length.
|
inline |
Gets the status of the automated SLAM process.
Initializes the SLAM pipeline.
It sets up the first point cloud. After that, registerPointCloud
can be called to perform the registration process.
void OCLSLAM< CR, CW >::registerPointCloud | ( | ) |
Registers a point cloud.
Runs ICP
on the current point cloud, delivers the result to OpenGL for visualization, and to OctoMap, in order to update the map.
|
inline |
Sets the variability threshold \(\epsilon\) for the guided filter performed on the Depth frame.
|
inline |
Sets the window radius \(r\) for the guided filter performed on the Depth frame.
|
inline |
Sets the scaling applied to the depth frame for processing with the guided filter.
|
inline |
Sets the status of the Depth Guided Filter.
|
inline |
Sets the variability threshold \(\epsilon\) for the guided filter performed on the RGB frame.
|
inline |
Sets the window radius \(r\) for the guided filter performed on the RGB frame.
|
inline |
Sets the status of the RGB Guided Filter.
|
inline |
Sets the angle threshold (in degrees) for the convergence check of the ICP.
|
inline |
Sets the maximum number of iterations considered for an ICP registration.
|
inline |
Sets the scaling applying to the deviations when computing matrix S
in the ICP algorithm, for managing floating point arithmetic issues.
|
inline |
Sets the translation threshold (in mm) for the convergence check of the ICP.
|
inline |
Sets the parameter \( \alpha \) used in the distance function for the RBC data structure.
|
inline |
Sets the status of the RGB normalization.
|
inline |
Sets the sensor's focal length.
void OCLSLAM< CR, CW >::setSLAMStatus | ( | bool | flag | ) |
Sets the status of the automated SLAM process.
|
inline |
Toggles the status of the Depth Guided Filter.
|
inline |
Toggles the status of the RGB Guided Filter.
|
inline |
Toggles the status of the RGB normalization.
void OCLSLAM< CR, CW >::toggleSLAMStatus | ( | ) |
Toggles the status of the automated SLAM process.
void OCLSLAM< CR, CW >::write | ( | std::string | filename = std::string ("map.ot") | ) |
Stores an occupancy map on disk.
[in] | filename | name for the map file [.ot] . |
void OCLSLAM< CR, CW >::writeBinary | ( | std::string | filename = std::string ("map.bt") | ) |
Stores an binary map on disk.
[in] | filename | name for the map file [.bt] . |
Represents the orietation with respect to the global coordinate frame, given in quaternion representation, \( \dot{q}_g = \left[ \begin{matrix} q_x & q_y & q_z & q_w \end{matrix} \right]^T \).
Represents the orietation with respect to the global coordinate frame, given in rotation matrix representation, \( R_g \).
Represents the scale of the current point cloud with respect to the first one, given as a scalar, \( s_g \). It's used when transforming the current point cloud before mapping.
std::function<void ()> OCLSLAM< CR, CW >::slam |
Performs the SLAM process.
Initially, it points to init
for registering the first point cloud, and after that it gets assigned with a function that runs repeatedly registerPointCloud
.
Represents the translation (in mm) with respect to the global coordinate frame, given as a vector in 3-D, \( t_g \).
Counts the discrete time steps, as in the number of point clouds registered.