I want to save the ESDF map generated by FIESTA. Is there a way to do it?

Hi,
I'm run cow_and_lady dataset and trying to use multi-threaded raycasting.
However, process keep crashes if ray_cast_num_thread param is any value positive integer.

In this log, ray_cast_num_thread was set to 6 but any positive value crashes the process.

Maximum possible thread is 24 for my desktop, fiesta is built with debug mode.

Following in Fiesta.h is modified slightly for output stream

template<class DepthMsgType, class PoseMsgType>
void Fiesta<DepthMsgType, PoseMsgType>::RaycastMultithread() {
     // TODO: when using vector, this is not needed
#ifdef HASH_TABLE
     set_free_.clear();
       set_occ_.clear();
#endif
     int tt = ++tot_;
     timing::Timer raycastingTimer("raycasting");

     if (parameters_.ray_cast_num_thread_==0) {
          RaycastProcess(0, cloud_.points.size(), tt);
     } else {
          int part = cloud_.points.size()/parameters_.ray_cast_num_thread_;
          
          std::list<std::thread> integration_threads = std::list<std::thread>();
          for (size_t i = 0; i < parameters_.ray_cast_num_thread_; ++i) {
               ROS_WARN_STREAM("part : " << part << " thread count : " << i);
               integration_threads.emplace_back(&Fiesta::RaycastProcess, this, i, part, tt);
          }
          for (std::thread &thread : integration_threads) {
               thread.join();
          }
     }
     raycastingTimer.Stop();
}

console output from cow & lady launched with GDB

$ roslaunch fiesta cow_and_lady.launch 
... logging to /home/seunghwan/.ros/log/e218fa0a-4c77-11ea-a841-00d8617c19f3/roslaunch-seunghwan-ryzen-desktop-14727.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

RLException: Invalid roslaunch XML syntax: not well-formed (invalid token): line 3, column 20
The traceback for the exception was written to the log file
seunghwan@seunghwan-ryzen-desktop:~/fast_ws/src/FIESTA$ roslaunch fiesta cow_and_lady.launch 
... logging to /home/seunghwan/.ros/log/f0c12334-4c77-11ea-a841-00d8617c19f3/roslaunch-seunghwan-ryzen-desktop-14761.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://seunghwan-ryzen-desktop:41235/

SUMMARY
========

PARAMETERS
 * /fiesta/depth_filter_margin: 0
 * /fiesta/depth_filter_max_dist: 10.0
 * /fiesta/depth_filter_min_dist: 0.1
 * /fiesta/depth_filter_tolerance: 0.1
 * /fiesta/global_map: True
 * /fiesta/global_update: True
 * /fiesta/global_vis: True
 * /fiesta/lx: -10.0
 * /fiesta/ly: -10.0
 * /fiesta/lz: -1
 * /fiesta/max_ray_length: 5.0
 * /fiesta/min_ray_length: 0.5
 * /fiesta/p_hit: 0.7
 * /fiesta/p_max: 0.97
 * /fiesta/p_min: 0.12
 * /fiesta/p_miss: 0.35
 * /fiesta/p_occ: 0.8
 * /fiesta/radius_x: 3.0
 * /fiesta/radius_y: 3.0
 * /fiesta/radius_z: 1.5
 * /fiesta/ray_cast_num_thread: 6
 * /fiesta/reserved_size: 1000000
 * /fiesta/resolution: 0.05
 * /fiesta/rx: 10.0
 * /fiesta/ry: 10.0
 * /fiesta/rz: 3
 * /fiesta/slice_vis_level: 1.6
 * /fiesta/slice_vis_max_dist: 2.0
 * /fiesta/update_esdf_every_n_sec: 0.1
 * /fiesta/use_depth_filter: True
 * /fiesta/vis_lower_bound: 0
 * /fiesta/vis_upper_bound: 10
 * /fiesta/visualize_every_n_updates: 10
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    fiesta (fiesta/test_fiesta)
    rvizvisualisation (rviz/rviz)

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

setting /run_id to f0c12334-4c77-11ea-a841-00d8617c19f3
process[rosout-1]: started with pid [14786]
started core service [/rosout]
process[fiesta-2]: started with pid [14793]
process[rvizvisualisation-3]: started with pid [14794]
GNU gdb (Ubuntu 8.1-0ubuntu3.2) 8.1.0.20180409-git
Copyright (C) 2018 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
<http://www.gnu.org/software/gdb/documentation/>.
For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from /home/seunghwan/fast_ws/devel/lib/fiesta/test_fiesta...done.
Starting program: /home/seunghwan/fast_ws/devel/lib/fiesta/test_fiesta \~depth:=/camera/depth_registered/points \~transform:=/kinect/vrpn_client/estimated_transform __name:=fiesta __log:=/home/seunghwan/.ros/log/f0c12334-4c77-11ea-a841-00d8617c19f3/fiesta-2.log
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[New Thread 0x7fffefb20700 (LWP 14811)]
[New Thread 0x7fffef31f700 (LWP 14812)]
[New Thread 0x7fffeeb1e700 (LWP 14813)]
[New Thread 0x7fffee31d700 (LWP 14814)]
12800000
[New Thread 0x7fffedb1c700 (LWP 14841)]
Pointcloud Size:	307200
[ WARN] [1581388869.453713184]: part : 51200 thread count : 0
[New Thread 0x7fffed31b700 (LWP 14856)]
[ WARN] [1581388869.454807722]: part : 51200 thread count : 1
[New Thread 0x7fffecb1a700 (LWP 14857)]
[ WARN] [1581388869.454914913]: part : 51200 thread count : 2
[New Thread 0x7fffe4f2a700 (LWP 14858)]
[ WARN] [1581388869.455101774]: part : 51200 thread count : 3
[New Thread 0x7fffcaa30700 (LWP 14859)]

Thread 8 "test_fiesta" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffecb1a700 (LWP 14857)]
std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement> (
    this=this@entry=0x5555557ed220, __args#0=...) at /usr/include/c++/7/bits/deque.tcc:167
167		    _Alloc_traits::construct(this->_M_impl,
(gdb) trace
Tracepoint 1 at 0x55555557fd8f: file /usr/include/eigen3/Eigen/src/Core/DenseStorage.h, line 194.

slightly modified cow_and_lady.launch

<launch>
    <node pkg="fiesta" type="test_fiesta" name="fiesta" output="screen" launch-prefix="gdb -ex run --args"
          required="true">

        <param name="resolution" value="0.05"/>
        <param name="update_esdf_every_n_sec" value="0.1"/>

        <!--Hash Table only-->
        <param name="reserved_size" value="1000000"/>

        <!--Array only-->
        <param name="lx" value="-10.0"/>
        <param name="ly" value="-10.0"/>
        <param name="lz" value="-1"/>
        <param name="rx" value="+10.0"/>
        <param name="ry" value="+10.0"/>
        <param name="rz" value="+3"/>

        <!-- raycasting parameters -->
        <param name="min_ray_length" value="0.5"/>
        <param name="max_ray_length" value="5.0"/>
        <!--If hash table is used, make sure the value is 0. In array implementation, 0 means no other thread.-->
        <param name="ray_cast_num_thread" value="6"/>



        <!-- needed when using depth image-->
        <!--        <param name="center_x" value="323.120483"/>-->
        <!--        <param name="center_y" value="236.743210"/>-->
        <!--        <param name="focal_x" value="385.754486"/>-->
        <!--        <param name="focal_y" value="385.754486"/>-->

        <!-- probabilistic grid map -->
        <param name="p_hit" value="0.70"/>
        <param name="p_miss" value="0.35"/>
        <param name="p_min" value="0.12"/>
        <param name="p_max" value="0.97"/>
        <param name="p_occ" value="0.80"/>

        <!-- global / local -->
        <param name="global_map" value="true"/>
        <param name="global_update" value="true"/>
        <param name="global_vis" value="true"/>
        <param name="radius_x" value="3.0"/>
        <param name="radius_y" value="3.0"/>
        <param name="radius_z" value="1.5"/>

        <!--depth_filter -->
        <param name="use_depth_filter" value="true"/>
        <param name="depth_filter_tolerance" value="0.1"/>
        <param name="depth_filter_max_dist" value="10.0"/>
        <param name="depth_filter_min_dist" value="0.1"/>
        <!--unit: pixel-->
        <param name="depth_filter_margin" value="0"/>

        <!-- visulization -->
        <!--0 for no visulize-->
        <param name="visualize_every_n_updates" value="10"/>
        <param name="slice_vis_max_dist" value="2.0"/>
        <!-- relative to the lz if array is used, relative to the origin if hash table is used -->
        <param name="slice_vis_level" value="1.6"/>
        <param name="vis_lower_bound" value="0"/>
        <param name="vis_upper_bound" value="+10"/>

        <!-- subsribe source -->
        <remap from="~depth" to="/camera/depth_registered/points"/>
        <remap from="~transform" to="/kinect/vrpn_client/estimated_transform"/>
    </node>

    <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find fiesta)/demo.rviz" />
</launch>

I has run FIESTA successfully, and then I run other project that maybe lead to the environment changed.But I can't find reason of the error.

I run the code:
catkin_make
source ~/catkin_ws/devel/setup.bash
roslaunch fiesta cow_and_lady.launch
rosbag play data.bag

I checke the version
PCL is 1.7.2
OPenCV is 3.4.3
Eigen 3.3.0

When the environment changes from indoor dense obstacles to outdoor open obstacles ( a few obstacles), distance_buffer_[idx] in GetSliceMarker is <0 or >=infinity_. Slice_pub is empty. How to modify code?

I am testing this program on ubuntu 18, Xavier NX. Any launch will crash with an segment fault and I cannot find why.

I used valgrind to test it, which outputs the log as follows:

==23003== Memcheck, a memory error detector
==23003== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==23003== Using Valgrind-3.13.0 and LibVEX; rerun with -h for copyright info
==23003== Command: /home/microhiggs/yzchen_ws/yzchen_ws_tx2/fiesta/devel/lib/fiesta/test_fiesta ~depth:=/camera/depth_registered/points ~transform:=/kinect/vrpn_client/estimated_transform __name:=fiesta __log:=/home/microhiggs/.ros/log/7710de6a-cb76-11eb-b46e-70665563cf6d/fiesta-1.log
==23003== Parent PID: 22987
==23003== 
==23003== Invalid read of size 8
==23003==    at 0x49AFF98: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x4A03DEB: ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x49EDA47: ros::spin() (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x12705F: main (in /home/microhiggs/yzchen_ws/yzchen_ws_tx2/fiesta/devel/lib/fiesta/test_fiesta)
==23003==  Address 0x18 is not stack'd, malloc'd or (recently) free'd
==23003== 
==23003== 
==23003== Process terminating with default action of signal 11 (SIGSEGV)
==23003==  Access not within mapped region at address 0x18
==23003==    at 0x49AFF98: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x4A03DEB: ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x49EDA47: ros::spin() (in /opt/ros/melodic/lib/libroscpp.so)
==23003==    by 0x12705F: main (in /home/microhiggs/yzchen_ws/yzchen_ws_tx2/fiesta/devel/lib/fiesta/test_fiesta)
==23003==  If you believe this happened as a result of a stack
==23003==  overflow in your program's main thread (unlikely but
==23003==  possible), you can try to increase the size of the
==23003==  main thread stack using the --main-stacksize= flag.
==23003==  The main thread stack size used in this run was 8388608.
==23003== 
==23003== HEAP SUMMARY:
==23003==     in use at exit: 60,222,044 bytes in 696 blocks
==23003==   total heap usage: 5,694 allocs, 4,998 frees, 60,656,379 bytes allocated
==23003== 
==23003== LEAK SUMMARY:
==23003==    definitely lost: 16 bytes in 1 blocks
==23003==    indirectly lost: 0 bytes in 0 blocks
==23003==      possibly lost: 101,407 bytes in 34 blocks
==23003==    still reachable: 60,120,621 bytes in 661 blocks
==23003==         suppressed: 0 bytes in 0 blocks
==23003== Rerun with --leak-check=full to see details of leaked memory
==23003== 
==23003== For counts of detected and suppressed errors, rerun with: -v
==23003== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)

Could anyone help? Thanks.

Do it.

I want to use the FIESTA mapping library without ROS. How can I feed the point cloud and pose information into the algorithm?

不需要精度很高，需要的是能支持无人机飞行百公里左右的建图内存消耗

Hello,
When compiling via catkin_make there is an error in the underlying linking between libpcl_kdtree and LZ4. Are there any thoughts as to what could cause this and how to correct? System is 18.04 running melodic.
/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/libpcl_kdtree.so: undefined reference to LZ4_resetStreamHC'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/libpcl_kdtree.so: undefined reference to LZ4_setStreamDecode' /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/libpcl_kdtree.so: undefined reference to LZ4_decompress_safe'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/libpcl_kdtree.so: undefined reference to LZ4_decompress_safe_continue' /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/libpcl_kdtree.so: undefined reference to LZ4_compress_HC_continue'
`

I am trying to run Fiesta with ORB-SLAM2 providing the pose information on the TUM rgbd dataset. And the mapping result is very poor when the pose is not accurate. I am wondering if there is any way to use the loop information from the ORB-SLAM2 to correct the mapping result. Thanks

When running the code, the current slicing value is not viewable with RVIZ. The following is reported.
scale.z of POINTS is ignored. Points should not be empty for specified marker type. Marker is fully transparent (color.a is 0.0).

Using fairly similar parameters to the demo.launch file.

it should be

roslaunch fiesta cow_and_lady.launch

rather than

roslaunch Fiesta cow_and_lady.launch

as the package name seems to be in lower case

Hi,

I have looked through your repos in TRR, FastPlanner and FIESTA with the following questions:

1. What is the fundamental differences betweet local planners approaches in these three repos?

2. In the TRR paper, you say that the local mapping is based on FIESTA, but the code says otherwise. Why?

3. Why does TRR and FIESTA use raycasting, while Fast Planner does not?

4. What kind of speedup does raycasting provide in real scenarios?

Thanks!

I have read the paper, there was an experiment on EuRoC datasets. And for EuRoC, how to obtain the depth information?

Dear author, I found it confusing in ESDFMap.h:24, where MatrixHash is defined. Why you designed the hash function like this, using transform as input to compute the hash value? What is the benefit?
Thanks.

How to use Lidar such as VLP16 as input？

Dear authors @hlx1996 @USTfgaoaa ,

Thanks for your open-source code. Great work!
I noticed in your TRR project, you use Dense Surfel Mapping(DSM) for global mapping while FIESTA for local mapping. After I separately test the two mapping modules, I found something interesting.

FIESTA has 3D probabilistic representation and ESDF, but seems has more error than DSM when VINS is used. See below pics for an illustration. The first picture is from FIESTA. Vins-mono is launched. The map with height color is from FIESTA, while the white pointcloud is from an RGBD camera (that you could think as a groundtruth pointcloud). The camera pose from VINS is good but there is an obvious discrepancy between the two pointclouds. The second pic is from DSM. From there you can see DSM does a really good job.

Also, it seems FIESTA does not integrate pose-graph from VINS side, leading to an accumulation of the mapping error, which may cause bad issues for map global consistency.

I am wondering if you guys have ever encountered those problems in your experiments.

Regarding to DSM, although it is more accurate, it seems DSM only has 3D deterministic surfel representation of the environment but no probabilistic property and ESDF available. So I am also wondering if you have plans to integrate the two mapping modules together to make a more robust, scalable, and accurate 3D mapping module.

Thanks!

when running the demo－"cow and lady",why do I meet this problem:
================================================================================REQUIRED process [fiesta-2] has died!
process has died [pid 11418, exit code -11, cmd /home/qjp/Backup/Lib/My_Fiesta/devel/lib/fiesta/test_fiesta ~depth:=/camera/depth_registered/points ~transform:=/kinect/vrpn_client/estimated_transform __name:=fiesta __log:=/home/qjp/.ros/log/369c6134-9cf0-11ea-8da5-d0577bdd6576/fiesta-2.log].
log file: /home/qjp/.ros/log/369c6134-9cf0-11ea-8da5-d0577bdd6576/fiesta-2*.log
Initiating shutdown!

in ESDFMap.cpp line 319.The loop is looking for the closet obstacle
break would run out the for loop,is that a mistake or my misunderstanding of the alg?

I've just built FIESTA on Ubuntu 18.04 with ROS Melodic and I'm getting very slow grid updates when running cow_and_lady example on a fast CPU . UpdateESDF text messages to terminal take a few seconds to advance. Perhaps RViz is really slow with PointCloud messages, which I observed on another occasion. Is there a headless (no display) test of FIESTA available?

Hi,
In the simulation,
it is often observed that quadrotor model follows trajectory undesirable way
that the vehicle flip towards wrong direction then return & starts to follow trajectory (See GIF below)

This easily can be observed especially when transitioning from stopped state(WAIT_TARGET) to executing new trajectory(EXEC_TRAJ) or rarely seen while following trajectory.

I'd like to know why and is this can easily be solved?

Thank you

P.S. Launch file & parameters are left untouched.

I've locked in Fiesta.h and there is no definition PointcloudCallback! Only the depth

作者您好

Voxblox的这种方法可以允许ESDF的地图动态改变, 使用存储在TSDF中的距离, 而不是从最近的occupied voxel中计算距离. 在原始代码中, 每个voxel有一个占据或者free的状态, 不能改变. voxblox将这个概念改变为around surface fixed band.

想問FIESTA未來會有解決動態環境問題的方法？