hkpolyu-uav/flvis

FLVIS: Feedback Loop Based Visual Inertial SLAM

flvis's Issues

<flvis/KeyFrame.h> is missing

Hi, and thank you for making this code available. I am trying to build it, and it looks like

KeyFrame.h
CorrectionInf.h

are missing. Should these headers be included? Where can i find them?

Thanks!

FLVIS Crashing when something is brought near to the camera

Flvis crashes when I bring my hand near to the camera. I am using d435i and pixhawk. Probably because there are no features for it to track. Can it be rectified by modifying any parameters in the config files? Would be grateful for guidance to find a solution for this!

Thank you!

: Failed to load nodelet [/TrackingNodeletClass_loader] of type [flvis/TrackingNodeletClass] even after refreshing the cache:

I have the following issues when running flvis.lauch files

wkyoun@wkyoun:~/catkin_ws/src/FLVIS/3rdPartLib$ roslaunch flvis flvis_bag.launch

... logging to /home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/roslaunch-wkyoun-3680.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/wkyoun/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://localhost:34925/

SUMMARY
========

PARAMETERS
 * /LocalMapNodeletClass_loader/window_size: 8
 * /flvis_nodelet_manager/num_worker_threads: 4
 * /rosdistro: melodic
 * /rosversion: 1.14.6
 * /voc: /home/wkyoun/catk...
 * /yamlconfigfile: /home/wkyoun/catk...

NODES
  /
    LocalMapNodeletClass_loader (nodelet/nodelet)
    LoopClosingNodeletClass_loader (nodelet/nodelet)
    TrackingNodeletClass_loader (nodelet/nodelet)
    flvis_nodelet_manager (nodelet/nodelet)
    rosbag (rosbag/play)

ROS_MASTER_URI=http://localhost:11311

process[rosbag-1]: started with pid [3695]
process[flvis_nodelet_manager-2]: started with pid [3696]
process[TrackingNodeletClass_loader-3]: started with pid [3699]
process[LocalMapNodeletClass_loader-4]: started with pid [3704]
process[LoopClosingNodeletClass_loader-5]: started with pid [3707]
[ INFO] [1628058548.304574097]: Loading nodelet /TrackingNodeletClass_loader of type flvis/TrackingNodeletClass to manager flvis_nodelet_manager with the following remappings:
[ INFO] [1628058548.306047645]: /imu -> /camera/imu
[ INFO] [1628058548.306077851]: /vo/input_image_0 -> /camera/infra1/image_rect_raw
[ INFO] [1628058548.306099428]: /vo/input_image_1 -> /camera/aligned_depth_to_infra1/image_raw
[ INFO] [1628058548.308495280]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1628058548.310410738]: Loading nodelet /LocalMapNodeletClass_loader of type flvis/LocalMapNodeletClass to manager flvis_nodelet_manager with the following remappings:
[ INFO] [1628058548.313362045]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1628058548.315448631]: Loading nodelet /LoopClosingNodeletClass_loader of type flvis/LoopClosingNodeletClass to manager flvis_nodelet_manager with the following remappings:
[ INFO] [1628058548.316921077]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1628058548.325498208]: Initializing nodelet with 4 worker threads.
[ INFO] [1628058548.329834500]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] is now available.
[ INFO] [1628058548.334870470]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] is now available.
[ INFO] [1628058548.338557993]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] is now available.

**[ERROR] [1628058548.348605360]: Failed to load nodelet [/TrackingNodeletClass_loader] of type [flvis/TrackingNodeletClass] even after refreshing the cache: Failed to load library /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)
[ERROR] [1628058548.348633783]: The error before refreshing the cache was: Failed to load library** /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)
[FATAL] [1628058548.348761256]: Failed to load nodelet '/TrackingNodeletClass_loader` of type `flvis/TrackingNodeletClass` to manager `flvis_nodelet_manager'
[ERROR] [1628058548.363892138]: Failed to load nodelet [/LocalMapNodeletClass_loader] of type [flvis/LocalMapNodeletClass] even after refreshing the cache: Failed to load library /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)**
[ERROR] [1628058548.363918278]: The error before refreshing the cache was: Failed to load library /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)
[FATAL] [1628058548.364013669]: Failed to load nodelet '/LocalMapNodeletClass_loader` of type `flvis/LocalMapNodeletClass` to manager `flvis_nodelet_manager'
[ERROR] [1628058548.379130266]: Failed to load nodelet [/LoopClosingNodeletClass_loader] of type [flvis/LoopClosingNodeletClass] even after refreshing the cache: Failed to load library /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)
[ERROR] [1628058548.379160131]: The error before refreshing the cache was: Failed to load library /home/wkyoun/catkin_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_opengl_helper.so: cannot open shared object file: No such file or directory)
[FATAL] [1628058548.379301086]: Failed to load nodelet '/LoopClosingNodeletClass_loader` of type `flvis/LoopClosingNodeletClass` to manager `flvis_nodelet_manager'
[TrackingNodeletClass_loader-3] process has died [pid 3699, exit code 255, cmd bash -c sleep 1.0; $0 $@ /opt/ros/melodic/lib/nodelet/nodelet load flvis/TrackingNodeletClass flvis_nodelet_manager /vo/input_image_0:=/camera/infra1/image_rect_raw /vo/input_image_1:=/camera/aligned_depth_to_infra1/image_raw /imu:=/camera/imu __name:=TrackingNodeletClass_loader __log:=/home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/TrackingNodeletClass_loader-3.log].
log file: /home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/TrackingNodeletClass_loader-3*.log
[LocalMapNodeletClass_loader-4] process has died [pid 3704, exit code 255, cmd bash -c sleep 1.0; $0 $@ /opt/ros/melodic/lib/nodelet/nodelet load flvis/LocalMapNodeletClass flvis_nodelet_manager __name:=LocalMapNodeletClass_loader __log:=/home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/LocalMapNodeletClass_loader-4.log].
log file: /home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/LocalMapNodeletClass_loader-4*.log
[LoopClosingNodeletClass_loader-5] process has died [pid 3707, exit code 255, cmd bash -c sleep 1.0; $0 $@ /opt/ros/melodic/lib/nodelet/nodelet load flvis/LoopClosingNodeletClass flvis_nodelet_manager __name:=LoopClosingNodeletClass_loader __log:=/home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/LoopClosingNodeletClass_loader-5.log].
log file: /home/wkyoun/.ros/log/e21125fe-f4e7-11eb-a66b-f83441d19d8b/LoopClosingNodeletClass_loader-5*.log
^C[flvis_nodelet_manager-2] killing on exit
[rosbag-1] killing on exit
shutting down processing monitor...''
... shutting down processing monitor complete
done

The program may exit early with an error when you play the bag at 2X speed

I just want to report the solutions to an issue when you test FLVIS with rosbag:
tracking: 112|119|156 tracking: 105|111|157 tracking: 102|116|166 tracking: 98|107|167 tracking: 81|87|147 tracking: 60|70|160 tracking: 65|70|155 tracking: 34|41|141 tracking: 43|104|140 tracking: 9|89|139 [Critical Warning] Tracking Fail-no enough lm pairs tracking: 7|63|141 [Critical Warning] Tracking Fail-no enough lm pairs Tracking failed! Swith to tracking Fail Mode vision tracking fail, IMU motion only Tring to recover~ [Critical Warning]: motion not in queue! please enlarge the buffer size /opt/ros/melodic/lib/nodelet/nodelet: line 1: 29821 Segmentation fault (core dumped) $0 $@ [flvis_nodelet_manager-3] process has died [pid 29819, exit code 139, cmd bash -c sleep 0; $0 $@ /opt/ros/melodic/lib/nodelet/nodelet manager __name:=flvis_nodelet_manager __log:=/home/zuzu/.ros/log/54a07096-71c2-11ec-8464-c8e2650adb10/flvis_nodelet_manager-3.log]. log file: /home/zuzu/.ros/log/54a07096-71c2-11ec-8464-c8e2650adb10/flvis_nodelet_manager-3*.log ^C[TrackingNodeletClass_loader-4] killing on exit [rosbag-2] killing on exit [ INFO] [1641784028.984848051]: Unloading nodelet /TrackingNodeletClass_loader from manager flvis_nodelet_manager [ INFO] [1641784028.985259802]: waitForService: Service [/flvis_nodelet_manager/unload_nodelet] could not connect to host [zuzu-MS-7C75:40729], waiting... [ WARN] [1641784028.985274043]: Couldn't find service flvis_nodelet_manager/unload_nodelet, perhaps the manager is already shut down
This issue happens when you use flvis with only stereo mode (no imu data is input). Two solutions can be worthy to try:
1. In the .yaml config file, tune the three parameters:
#goodFeaturesToTrack detector maxCorners
feature_para4: 500 (increase it, i.g to 1000)
#goodFeaturesToTrack detector qualityLevel
feature_para5: 0.005 (decrease)
#goodFeaturesToTrack detector minDistance
feature_para6: 5 (decrease)
These params can help to detect more features, while the requirements for the feature quality may be loosened.
2. Play the bag at a lower speed (the default speed in the launch file is 2.0 times faster than the original speed).

Large drift when using IMU only

Hi,

I have the code running well on a D455, but I notice that when visual tracking is lost, the pose will drift quickly away from truth.

For example if the camera is static, and I block the cameras, the pose immediately drifts fast away.

Is there a way to improve the IMU-only tracking so that it stays reasonably accurate when visual points are lost?

Thanks!

Current frame being used twice in viCorrectionFromVision

This line calls my attention since I would think that last_frame should be used instead of curr_frame. Could this be a bug or I am missing something?

How to get covariance matrix ?

Hi @pattylo,

I want to find out the covariance matrix. but I am confuse where to add it,

should I change the default filter of madgwick and add ekf to get covariance or do you suggest any better way ?

thank you.

Error occur when I run "roslaunch flvis flvis_bag.launch"

Hi,
I'm trying to run flvis using realsense d435i. Before this, I got some error when I run flvis_bag.launch.

Do you have some solution?

I'm using ros noetic.

... logging to /home/dongheon/.ros/log/f69272aa-acbd-11ed-a9a2-53836cc1c798/roslaunch-ubuntu-1693277.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ubuntu:34779/

SUMMARY
========

PARAMETERS
 * /flvis_nodelet_manager/num_worker_threads: 4
 * /rosdistro: noetic
 * /rosversion: 1.15.15
 * /voc: /home/dongheon/pr...
 * /yamlconfigfile: /home/dongheon/pr...

NODES
  /
    LoopClosingNodeletClass_loader (nodelet/nodelet)
    TrackingNodeletClass_loader (nodelet/nodelet)
    flvis_nodelet_manager (nodelet/nodelet)
    rosbag (rosbag/play)

auto-starting new master
process[master]: started with pid [1693285]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to f69272aa-acbd-11ed-a9a2-53836cc1c798
process[rosout-1]: started with pid [1693295]
started core service [/rosout]
process[rosbag-2]: started with pid [1693298]
process[flvis_nodelet_manager-3]: started with pid [1693303]
process[TrackingNodeletClass_loader-4]: started with pid [1693305]
process[LoopClosingNodeletClass_loader-5]: started with pid [1693307]
[ INFO] [1676416742.123658438]: Loading nodelet /TrackingNodeletClass_loader of type flvis/TrackingNodeletClass to manager flvis_nodelet_manager with the following remappings:
[ INFO] [1676416742.125244006]: Loading nodelet /LoopClosingNodeletClass_loader of type flvis/LoopClosingNodeletClass to manager flvis_nodelet_manager with the following remappings:
[ INFO] [1676416742.125777490]: /imu -> /camera/imu
[ INFO] [1676416742.125824069]: /vo/input_image_0 -> /camera/infra1/image_rect_raw
[ INFO] [1676416742.125862332]: /vo/input_image_1 -> /camera/aligned_depth_to_infra1/image_raw
[ INFO] [1676416742.128011097]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1676416742.129707252]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1676416742.152293621]: Initializing nodelet with 4 worker threads.
[ INFO] [1676416742.170086540]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] is now available.
[ INFO] [1676416742.171106915]: waitForService: Service [/flvis_nodelet_manager/load_nodelet] is now available.
[ERROR] [1676416742.196321659]: Failed to load nodelet [/TrackingNodeletClass_loader] of type [flvis/TrackingNodeletClass] even after refreshing the cache: Failed to load library /home/dongheon/projects/mlmapping/flvis_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libopencv_core.so.3.4: cannot open shared object file: No such file or directory)
[ERROR] [1676416742.196371383]: The error before refreshing the cache was: Failed to load library /home/dongheon/projects/mlmapping/flvis_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libopencv_core.so.3.4: cannot open shared object file: No such file or directory)
[FATAL] [1676416742.196522860]: Failed to load nodelet '/TrackingNodeletClass_loader` of type `flvis/TrackingNodeletClass` to manager `flvis_nodelet_manager'
[ERROR] [1676416742.217761532]: Failed to load nodelet [/LoopClosingNodeletClass_loader] of type [flvis/LoopClosingNodeletClass] even after refreshing the cache: Failed to load library /home/dongheon/projects/mlmapping/flvis_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libopencv_core.so.3.4: cannot open shared object file: No such file or directory)
[ERROR] [1676416742.217824182]: The error before refreshing the cache was: Failed to load library /home/dongheon/projects/mlmapping/flvis_ws/devel/lib//libflvis.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libopencv_core.so.3.4: cannot open shared object file: No such file or directory)
[FATAL] [1676416742.218977881]: Failed to load nodelet '/LoopClosingNodeletClass_loader` of type `flvis/LoopClosingNodeletClass` to manager `flvis_nodelet_manager'
[TrackingNodeletClass_loader-4] process has died [pid 1693305, exit code 255, cmd bash -c sleep 1.0; $0 $@ /opt/ros/noetic/lib/nodelet/nodelet load flvis/TrackingNodeletClass flvis_nodelet_manager /vo/input_image_0:=/camera/infra1/image_rect_raw /vo/input_image_1:=/camera/aligned_depth_to_infra1/image_raw /imu:=/camera/imu __name:=TrackingNodeletClass_loader __log:=/home/dongheon/.ros/log/f69272aa-acbd-11ed-a9a2-53836cc1c798/TrackingNodeletClass_loader-4.log].
log file: /home/dongheon/.ros/log/f69272aa-acbd-11ed-a9a2-53836cc1c798/TrackingNodeletClass_loader-4*.log
[LoopClosingNodeletClass_loader-5] process has died [pid 1693307, exit code 255, cmd bash -c sleep 1.0; $0 $@ /opt/ros/noetic/lib/nodelet/nodelet load flvis/LoopClosingNodeletClass flvis_nodelet_manager __name:=LoopClosingNodeletClass_loader __log:=/home/dongheon/.ros/log/f69272aa-acbd-11ed-a9a2-53836cc1c798/LoopClosingNodeletClass_loader-5.log].
log file: /home/dongheon/.ros/log/f69272aa-acbd-11ed-a9a2-53836cc1c798/LoopClosingNodeletClass_loader-5*.log

Attempt to initialize an empty graph Segmentation fault (core dumped)

First of all, thank you for sharing this work. I am evaluating different VSLAM for my research and I encounter the following error:

tracking: 48|50|113
tracking: 67|67|85
tracking: 61|63|82
tracking: 60|63|79
tracking: 58|60|79
tracking: 58|59|76
tracking: 65|86|95
tracking: 62|95|107
tracking: 66|66|66
tracking: 64|65|65
tracking: 62|63|63
tracking: 61|63|63
tracking: 62|63|63
tracking: 60|62|63
tracking: 53|62|67
tracking: 51|121|136
tracking: 42|139|151
virtual bool g2o::SparseOptimizer::initializeOptimization(g2o::HyperGraph::VertexSet&, int): Attempt to initialize an empty graph
/opt/ros/kinetic/lib/nodelet/nodelet: line 1:  7929 Segmentation fault      (core dumped) $0 $@
[flvis_nodelet_manager-3] process has died [pid 7927, exit code 139, cmd bash -c sleep 0; $0 $@ /opt/ros/kinetic/lib/nodelet/nodelet manager __name:=flvis_nodelet_manager __log:=/home/fabrice/.ros/log/bd569f54-0f88-11eb-9699-88b1116f3669/flvis_nodelet_manager-3.log].
log file: /home/fabrice/.ros/log/bd569f54-0f88-11eb-9699-88b1116f3669/flvis_nodelet_manager-3*.log
[ INFO] [1602836582.269961107]: Bond broken, exiting
[ INFO] [1602836582.366583719]: Bond broken, exiting
[ INFO] [1602836582.374452665]: Bond broken, exiting
[LoopClosingNodeletClass_loader-6] process has finished cleanly
log file: /home/fabrice/.ros/log/bd569f54-0f88-11eb-9699-88b1116f3669/Loo
pClosingNodeletClass_loader-6*.log
[TrackingNodeletClass_loader-4] process has finished cleanly

Do you have any idea why it happens? Or where do I have to dig to understand what is going on?

I am running FLVIS on ubuntu 16.04

Build error 'fatal error: nav_msgs/Odometry.h: No such file or directory'

Build error 'fatal error: nav_msgs/Odometry.h: No such file or directory' happens when it is build for the first time.

I found a solution as below:
In the CMakeList.txt of MLMapping folder, I added the following sentences:

In find_package(), nav_msgs, visualization_msgs and pcl_ros were added.
In generate_messages(), nav_msgs and visualization_msgs were added.
After that, such errors disappeared and its build succeeded.

FYI, this issue is similar to the following in MLMapping:
HKPolyU-UAV/MLMapping#3

Loosely or tightly-coupled?

Would you say that your estimator is a loosely or a tightly-coupled approach? I guess it is tightly-coupled but I am not really sure because it is far different from the others.

Is this suitable for mobile robots with only three degree of freedom?

Because there is little bit change of pitch and roll on mobile robot, the visual estimation with orientation feedforward (in purple) may not work. Am I right?

The state starts drifting when FLVIS is running and left stationary for a few minutes

Hello,
I have FLVIS running and if I leave it stationary for about 5-6 minutes it gradually starts drifting. There is no movement in front of the camera and ample features are visible. Would be grateful to receive guidance to rectify this.

Thank you so much in advance!

Full edge remotion in optimization in frame

I am running your system (thank you for sharing it) on my own data and can't avoid this problem of full edge remotion while performing the in frame optimization:

virtual bool g2o::SparseOptimizer::initializeOptimization(g2o::HyperGraph::VertexSet&, int): Attempt to initialize an empty graph
/opt/ros/kinetic/lib/nodelet/nodelet: line 1:   289 Segmentation fault      (core dumped) $0 $@

Do you have any idea of what could be causing this problem? I have invested more than a week checking the code and trying to figure out how to solve it but none of the ideas that I came up with helped me out.

Could this be a bug?

https://github.com/Ttoto/FLVIS/blob/7e83b3bf42a32de3637d9b5f05ec99ff63020f0e/src/frontend/vi_motion.cpp#L328

I would have expected this line to be something like: gyro_bias = (1-para_4)*gyro_bias+(para_4)*gyro_bias_est;. Could you please confirm if this is a mistake or not?

[FATAL] [1632737727.144338183]: Failed to load nodelet '/LoopClosingNodeletClass_loader` of type `flvis/LoopClosingNodeletClass` to manager `flvis_nodelet_manager'

While testing FLVIS, I faced this issue, where I am not able to load the loop closing nodelet, while the other nodelets are able to load. Any ideas why? The program crashes in runtime.
I am running it on ROS melodic.

Coordinate Transform and VIsualization

solvePnPRansac,
cv::solvePnPRansac(p3d,p2d,this->d_camera.K0_rect,this->d_camera.D0_rect,r_,t_,false,100,3.0,0.99,inliers,cv::SOLVEPNP_ITERATIVE);
r_ & t_ obtained by solvePnPRansac() is the rotation and translation that transform a 3d point in the world frame to camera frame. Following the conventions in State Estimation for Robotics by Prof. Timothy Barfoot : T_c_w transform a point in the world frame to the camera frame.
Pc=R_c_w*Pw+t_c_w。

From another point of view, T_c_w represents the pose of the world frame in the camera frame(transform (1,1,1)T in the world frame to camera frame by T_c_w*(1,1,1)_w).

So if we want to visualize the camera pose in the world frame, it should be T_w_c. In the code implementation, we only need to save one of T_c_w & T_w_c, since T_c_w = T_w_c.inverse(). solvePnP gives us T_c_w, so we kept it. And T_c_w is also used in Bundle Adjustment.

@todo
For visualization, in Rviz class, we need camera pose in world frame, it should be T_w_c, instead of T_c_w by the above functions. One can imagine how to the camera pose in the world frame is the vector (1,1,1)_c's coordinate in the world frame. SoViusalization ModuleThis func: void RVIZPath::pubPathT_c_w(const SE3 T_c_w, const ros::Time stamp) should be deleted. We should directly call this func: void RVIZPath::pubPathT_w_c(const SE3 T_w_c, const ros::Time stamp),

Where does loop closing feed back to the algorithm?

Hi, I have this code running on Windows, and have removed the ROS dependency. It runs very well, but I cannot see how the loop closing feeds back to the local map, or the tracking.

I can see that the local mapping feeds back to the tracking here:

void F2FTracking::correction_feed(const double time, const CorrectionInfStruct corr)
{
  this->correction_inf = corr;
  this->has_localmap_feedback = true;
}

After the loop closing thread does global optimization, how does this feed back to adjust the local map / tracking?

Thank you!