←  Xvisio Features and Quick Start
←  Xvisio SDK Documentation Home Page

VSLAM

1. Overview

This section mainly introduces the VSLAM function and how to quick start a VSLAM programming by using XVSDK.

2. Features

2.1 VSLAM Work Mode

Xvisio SLAM supports two modes: MIX mode (parts of the SLAM algorithm run on the Xvisio device side, and the others run on the host side), and Edge mode (the SLAM algorithm runs entirely on the device side).
User can select and switch modes freely according to the scenarios. Generally, most of the scenarios are suitable for mix mode, and edge mode can be used in some CPU environments with weak capabilities.

2.2 VSLAM Center Point

VSLAM is centered on the IMU of Xvisio device. After SLAM starting, a VSLAM coordinate system will be established which based on the device's gravity direction. The origin of the coordinate system is on IMU when VSLAM starts each time. The value of x,y,z is 0 when VSLAM starts. Pitch/yaw/roll represents the rotation of IMU device.
Start the device horizontally:
image
Tilt the device to start:
image

2.3 VSLAM Coordinate

Xvisio VSLAM coordinate is default of right-hand. X-axis is in the right positive direction, Y-axis is in the downward positive direction, and the Z-axis is in the forward positive direction. The schematic diagram is shown as follows:
image

2.4 6DOF
2.4.1 6DOF Pose Sturcture

6DOF Pose supports 3 formats (Eulerian angle, rotation matrix and quaternion) to express rotation.The 6DOF structure is shown as below:

struct Pose : public details::PosePred_<double> {
    Pose();
    /**
    * @brief Construct a pose with a translation, rotation, timestamps and confidence.
    */
    Pose(Vector3d const& translation, Matrix3d const& rotation,
          double hostTimestamp = std::numeric_limits<double>::infinity(), std::int64_t edgeTimestamp = (std::numeric_limits<std::int64_t>::min)(), double c=0.);
    /**
     * @brief Prediction of the pose based on angular and linear velocity and acceleration.
     * @param dt amount of prediction (in s)
     * @return The predicted (2nd order extrapolation) of the orientation.
     */
    Pose prediction(double dt) const;
};

The time base of hostTimestamp is the host side. Start timing (0) from the host side boot.
The time base of deviceTimestamp is the device side. Start timing (0) from device boot.
confidence indicates the trustworthy level of 6DOF. Value 0 means slam lost and value 1 means the highest confidence.

2.4.2 Get 6DOF

Xvisio VSLAM supports three ways to get 6DOF:

  1. Callback
    Register the callback function firstly,SDK will trigger the callback actively according to the frame rate and transmit 6DOF data. It has a simple process which can be used for applications with general real-time requirements.
    device->slam()->registerCallback( poseCallback );
    The frame rate of callback is generally determined by the frame rate of IMU (6DOF uses IMU for fusion).
  2. getpose()
    The application actively calls the interface function to obtain the latest 6DOF pose.
    This calling method has a high real-time requirements, which can obtain the latest real-time pose from the application design process.
    bool getPose(Pose &pose, double prediction) ;
    User can call this interface to actively obtain 6DOF data and set the prediction time at the same time. It is recommended to set the prediction time less than 0.016 (16ms, if the prediction is too large, 6dof prediction may not be accurate).
  3. getposeAt()
    The application calls the interface function actively to obtain the 6DOF pose of the specified timestamp, which is usually used for synchronization function.For example, transmitting the timestamp of the RGB frame can get the pose of RGB image.
    bool getPoseAt(Pose& pose, double timestamp);
2.5 VSLAM Mapping Mode
2.5.1 VIO

VIO mode does not include map loopclosure. As the odometer increases, the cumulative error will increase. It is default of VIO mode when VSLAM starts. User can use below interfaces to obtain VIO function:

bool start();
bool stop();
int registerCallback(std::function<void (xv::Pose const&)>);
bool unregisterCallback(int callbackId);
bool getPose(Pose &pose, double prediction) ;

Interface calling process is as follows:

  1. Register lost callback.
  2. Call start() to start SLAM.
  3. Call getPose（）to get 6DOF.
  4. Call stop() to stop SLAM.
    Refer demo-api.cpp（case 2，case3）for detail code.demo-api 。
2.5.2 Online Loopclosure

There is no need to build a map in advance. Build a map online and detect whether it is closed-loop in the background in real-time. If it is closed-loop, loopclosure optimization will be done. The disadvantage is that the map is only saved in memory and cannot be reused after restarting the device.
The calling interface of online loopclosure is similar to VIO mode except the setting difference. The interfaces used are as follows:

bool start();
bool stop();
bool xv::SlamEx::setEnableOnlineLoopClosure(bool enable);
int registerCallback(std::function<void (xv::Pose const&)>);
bool unregisterCallback(int callbackId);
bool getPose(Pose &pose, double prediction) ;

Interface calling process is as follows:

  1. Register lost callback.
  2. Call setEnableOnlineLoopClosure(true) to enable Online LoopClosure.
  3. Call start() to start SLAM.
  4. Call getPose（）to get 6DOF.
  5. Call stop() to stop SLAM.
2.5.3 CSLAM

The following interfaces are mainly used to achieve CSLAM function:

bool start();
bool stop();
bool loadMapAndSwitchToCslam(std::streambuf &mapStream, std::function<void (int)> done_callback, std::function<void (float)> localized_on_reference_map);
bool saveMapAndSwitchToCslam(std::streambuf &mapStream, std::function<void (int, int)> done_callback, std::function<void (float)> localized_on_reference_map);

Steps to Build Map:
To build a good map, firstly you should consider how to use the map. The map must contain all the viewpoints required by the application. If the final application is in another room, there is no reason to record the map in the current room. Similarly, if the final application displays some virtual objects on the ground, no need to record the other view of the room. The path to be moved by the application should be the final path of the recorded map. In order to ensure a good loop closure, walk on the same path twice: for example, start from a starting point, walk away, return to the starting point, then walk away on the same path again, and then return to the starting point. During the recording process, walking twice on the same path ensures a good overlap between the different recording viewpoints of the loop closure detection. During recording, it is important to avoid moving fast or facing areas with no features.
Sample:
Refer to demo-api.cpp（case 20，case21） for detail code.demo-api
The following is the API call process and schematic diagram, which are introduced according to two usage scenarios:
Sample1. Switch to cslam after mapping

API call process：

  1. VSLAM start()
    device->slam()->start(xv::Slam::Mode::Mixed);
    No need to register if user doesn't get 6DOF by callback.
    device->slam()->registerCallback( poseCallback );
  2. Call saveMapAndSwitchToCslam to save map automatically and switch to CSLAM. Transmit parameters map stream，function done_callback function and function localized_on_reference_map are needed. For example:
    device->slam()->saveMapAndSwitchToCslam(mapStream,cslamSavedCallback, cslamLocalizedCallback);
    Refer to C++ Application Interfaces for detail information of saveMapAndSwitchToCslam.
  3. 6DOF acquisition will take effect at the same time during map building 6DOF callback registered will be triggered, or 6DOF can be obtained through getPose().
  4. Call stop()，stop cslam
    device->slam()->stop();

Sample 2：Switch to CSLAM after loading the map

  1. Start VSLAM:register 6DOF callback（register before get 6DOF by callback).
    device->slam()->start(xv::Slam::Mode::Mixed);
    device->slam()->registerCallback( poseCallback );
  2. Call loadMapAndSwitchToCslam to load map automatically and switch to CSLAM.Transmit parameters map stream，function done_callback and function localized_on_reference_map. For example:
    device->slam()->loadMapAndSwitchToCslam(mapStream,cslamSwitchedCallback, cslamLocalizedCallback);
    Refer to C++ application interfaces for detail information of loadMapAndSwitchToCslam.
  3. 6DOF acquisition will take effect at the same time during map building.6DOF callback registered will be triggered, or 6DOF can be obtained through getPose().
  4. Call stop() to stop CSLAM.
    device->slam()->stop();

3. VSLAM Code:

  • Define Xvisio Device:
    std::shared_ptr<xv::Device> device = nullptr;
  • Read device_list, set timeout 10 seconds (recommended value which can also be shortened):
    auto device_list = xv::getDevices(10., "");
  • Judge whether the obtained devices are empty. It means failure if it is empty:
    if (device_list.empty())`
{
        LOG(LOG_Function,"initDevice faiiled:Timeout for device detection.");
        return false;
}
  • Get device（only for single equipment, please refer to the corresponding documentation for multiple equipment）
    device = device_list.begin()->second;
  • Start Online LoopClosure. It is default to VIO mode if it doesn't call this command.
    std::static_pointer_cast<xv::SlamEx>(device->slam())->setEnableOnlineLoopClosure(true);
  • Start VSLAM in MIX mode
    device->slam()->start(xv::Slam::Mode::Mixed);
  • Start VSLAM in Edge mode
    device->slam()->start(xv::Slam::Mode::Edge);
  • Use callback to get 6DOF pose
    int poseId = -1;
poseId = device->slam()->registerCallback([](const xv::Pose& pose){
    ...  
    });
  • Get 6DOF pose by getPose(). The prediction time is 10ms.
    double prediction = 0.010;
bool ok = device->slam()->getPose( pose, prediction );
if ( !ok ) {
...//failed.
}
  • Get 6DOF pose by getPoseAt(). The specified timestamp is pseudocode 'spec_TS'.
    double t = spec_TS;
xv::Pose poseAt;
bool ok = device->slam()->getPoseAt( poseAt, t );
if ( !ok ) {
...//failed.
}
  • CSlam Mapping
    std::filebuf mapStream;`
if (mapStream.open(map_filename, std::ios::binary | std::ios::out | std::ios::trunc) == nullptr) {
                std::cout << "open " << map_filename << " failed." << std::endl;
                break;
            }
device->slam()->saveMapAndSwitchToCslam(mapStream, cslamSavedCallback, cslamLocalizedCallback);
  • Register callback:
    void cslamSavedCallback(int status_of_saved_map, int map_quality)
{
    std::cout << " Save map (quality is " << map_quality << "/100) and switch to CSlam:";
    switch (status_of_saved_map)
    {
        case  2: std::cout << " Map well saved. " << std::endl; break;
        case -1: std::cout << " Map cannot be saved, an error occured when trying to save it." << std::endl; break;
        default: std::cout << " Unrecognized status of saved map " << std::endl; break;
    }
    mapStream.close();    
}       
void cslamLocalizedCallback(float percent)
{
    static int k = 0;
    if (k++ % 100 == 0) {
        localized_on_reference_percent = static_cast<int>(percent * 100);
        std::cout << "localized: " << localized_on_reference_percent << "%" << std::endl;
    }
}
  • CSlam load map
    device->slam()->loadMapAndSwitchToCslam(
                mapStream,
                cslamSwitchedCallback,
                cslamLocalizedCallback
            );
  • Register callback:
    void cslamSwitchedCallback(int map_quality)
{
    std::cout << " map (quality is " << map_quality << "/100) and switch to CSlam:";
    mapStream.close();
}
void cslamLocalizedCallback(float percent)
{
    static int k = 0;
    if (k++ % 100 == 0) {
        localized_on_reference_percent = static_cast<int>(percent * 100);
        std::cout << "localized: " << localized_on_reference_percent << "%" << std::endl;
    }
}
  • Stop SLAM
    The poseId here is the value assigned when registering callback.
    device->slam()->unregisterCallback(poseId);
device->slam()->stop();

←  Xvisio Features and Quick Start
←  Xvisio SDK Documentation Home Page
≡