Multi-threaded raycast crashes the process

[SwiftGust]

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>

[SwiftGust]

Running node with valgrind allows running without crash for a while but also ended up with crash

Pointcloud Size:	307200
[ WARN] [1581390544.891322981]: part : 12800 thread count : 0
[ WARN] [1581390544.891889122]: part : 12800 thread count : 1
[ WARN] [1581390544.917094175]: part : 12800 thread count : 2
[ WARN] [1581390544.917672915]: part : 12800 thread count : 3
[ WARN] [1581390544.961947186]: part : 12800 thread count : 4
[ WARN] [1581390545.030535443]: part : 12800 thread count : 5
[ WARN] [1581390545.045782456]: part : 12800 thread count : 6
[ WARN] [1581390545.133082881]: part : 12800 thread count : 7
[ WARN] [1581390545.201858416]: part : 12800 thread count : 8
[ WARN] [1581390545.320145490]: part : 12800 thread count : 9
[ WARN] [1581390545.388955013]: part : 12800 thread count : 10
[ WARN] [1581390545.424394160]: part : 12800 thread count : 11
[ WARN] [1581390545.485571163]: part : 12800 thread count : 12
[ WARN] [1581390545.501256590]: part : 12800 thread count : 13
[ WARN] [1581390545.581681769]: part : 12800 thread count : 14
[ WARN] [1581390545.658326267]: part : 12800 thread count : 15
[ WARN] [1581390545.735362051]: part : 12800 thread count : 16
[ WARN] [1581390545.802083365]: part : 12800 thread count : 17
[ WARN] [1581390545.845123844]: part : 12800 thread count : 18
[ WARN] [1581390545.865940586]: part : 12800 thread count : 19
[ WARN] [1581390545.889662891]: part : 12800 thread count : 20
[ WARN] [1581390545.905563252]: part : 12800 thread count : 21
[ WARN] [1581390545.926314167]: part : 12800 thread count : 22
[ WARN] [1581390545.941565189]: part : 12800 thread count : 23
Running 9 updates.
Occupancy Update 61829	Insert 112	Delete 22
Expanding 11051 nodes, with change_num = 431, accumulator = 296565
SM Timing
-----------
UpdateESDF	      9	03.621765	(00.402418 +- 00.272807)	[00.033322,01.034478]
raycasting	      9	09.633178	(01.070353 +- 00.069487)	[00.992591,01.247856]

==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 
==14181== Invalid write of size 4
==14181==    at 0x133D96: DenseStorage (DenseStorage.h:194)
==14181==    by 0x133D96: PlainObjectBase (PlainObjectBase.h:504)
==14181==    by 0x133D96: Matrix (Matrix.h:274)
==14181==    by 0x133D96: QueueElement (ESDFMap.h:39)
==14181==    by 0x133D96: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133D96: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133D96: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:167)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Address 0x3dea6030 is 1,440 bytes inside a block of size 1,536 free'd
==14181==    at 0x4C3123B: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B428: deallocate (new_allocator.h:125)
==14181==    by 0x12B428: deallocate (alloc_traits.h:462)
==14181==    by 0x12B428: _M_deallocate (stl_vector.h:180)
==14181==    by 0x12B428: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:448)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Block was alloc'd at
==14181==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B353: allocate (new_allocator.h:111)
==14181==    by 0x12B353: allocate (alloc_traits.h:436)
==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 
==14181== Invalid write of size 4
==14181==    at 0x133D9B: assignCoeff (AssignmentFunctors.h:24)
==14181==    by 0x133D9B: assignCoeff (AssignEvaluator.h:631)
==14181==    by 0x133D9B: assignCoeffByOuterInner (AssignEvaluator.h:645)
==14181==    by 0x133D9B: run (AssignEvaluator.h:206)
==14181==    by 0x133D9B: run (AssignEvaluator.h:435)
==14181==    by 0x133D9B: call_dense_assignment_loop<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:741)
==14181==    by 0x133D9B: run (AssignEvaluator.h:879)
==14181==    by 0x133D9B: call_assignment_no_alias<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:836)
==14181==    by 0x133D9B: _set_noalias<Eigen::Matrix<int, 3, 1> > (PlainObjectBase.h:728)
==14181==    by 0x133D9B: Matrix (Matrix.h:278)
==14181==    by 0x133D9B: QueueElement (ESDFMap.h:39)
==14181==    by 0x133D9B: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133D9B: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133D9B: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:167)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Address 0x3dea6028 is 1,432 bytes inside a block of size 1,536 free'd
==14181==    at 0x4C3123B: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B428: deallocate (new_allocator.h:125)
==14181==    by 0x12B428: deallocate (alloc_traits.h:462)
==14181==    by 0x12B428: _M_deallocate (stl_vector.h:180)
==14181==    by 0x12B428: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:448)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Block was alloc'd at
==14181==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B353: allocate (new_allocator.h:111)
==14181==    by 0x12B353: allocate (alloc_traits.h:436)
==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 
==14181== Invalid write of size 4
==14181==    at 0x133DA1: assignCoeff (AssignmentFunctors.h:24)
==14181==    by 0x133DA1: assignCoeff (AssignEvaluator.h:631)
==14181==    by 0x133DA1: assignCoeffByOuterInner (AssignEvaluator.h:645)
==14181==    by 0x133DA1: run (AssignEvaluator.h:206)
==14181==    by 0x133DA1: run (AssignEvaluator.h:207)
==14181==    by 0x133DA1: run (AssignEvaluator.h:435)
==14181==    by 0x133DA1: call_dense_assignment_loop<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:741)
==14181==    by 0x133DA1: run (AssignEvaluator.h:879)
==14181==    by 0x133DA1: call_assignment_no_alias<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:836)
==14181==    by 0x133DA1: _set_noalias<Eigen::Matrix<int, 3, 1> > (PlainObjectBase.h:728)
==14181==    by 0x133DA1: Matrix (Matrix.h:278)
==14181==    by 0x133DA1: QueueElement (ESDFMap.h:39)
==14181==    by 0x133DA1: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133DA1: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133DA1: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:167)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Address 0x3dea602c is 1,436 bytes inside a block of size 1,536 free'd
==14181==    at 0x4C3123B: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B428: deallocate (new_allocator.h:125)
==14181==    by 0x12B428: deallocate (alloc_traits.h:462)
==14181==    by 0x12B428: _M_deallocate (stl_vector.h:180)
==14181==    by 0x12B428: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:448)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Block was alloc'd at
==14181==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B353: allocate (new_allocator.h:111)
==14181==    by 0x12B353: allocate (alloc_traits.h:436)
==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 
==14181== Invalid write of size 8
==14181==    at 0x133DA7: QueueElement (ESDFMap.h:39)
==14181==    by 0x133DA7: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133DA7: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133DA7: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:167)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Address 0x3dea6038 is 1,448 bytes inside a block of size 1,536 free'd
==14181==    at 0x4C3123B: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B428: deallocate (new_allocator.h:125)
==14181==    by 0x12B428: deallocate (alloc_traits.h:462)
==14181==    by 0x12B428: _M_deallocate (stl_vector.h:180)
==14181==    by 0x12B428: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:448)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Block was alloc'd at
==14181==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B353: allocate (new_allocator.h:111)
==14181==    by 0x12B353: allocate (alloc_traits.h:436)
==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 
==14181== Invalid write of size 4
==14181==    at 0x133DAC: assignCoeff (AssignmentFunctors.h:24)
==14181==    by 0x133DAC: assignCoeff (AssignEvaluator.h:631)
==14181==    by 0x133DAC: assignCoeffByOuterInner (AssignEvaluator.h:645)
==14181==    by 0x133DAC: run (AssignEvaluator.h:206)
==14181==    by 0x133DAC: run (AssignEvaluator.h:207)
==14181==    by 0x133DAC: run (AssignEvaluator.h:207)
==14181==    by 0x133DAC: run (AssignEvaluator.h:435)
==14181==    by 0x133DAC: call_dense_assignment_loop<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:741)
==14181==    by 0x133DAC: run (AssignEvaluator.h:879)
==14181==    by 0x133DAC: call_assignment_no_alias<Eigen::Matrix<int, 3, 1>, Eigen::Matrix<int, 3, 1>, Eigen::internal::assign_op<int, int> > (AssignEvaluator.h:836)
==14181==    by 0x133DAC: _set_noalias<Eigen::Matrix<int, 3, 1> > (PlainObjectBase.h:728)
==14181==    by 0x133DAC: Matrix (Matrix.h:278)
==14181==    by 0x133DAC: QueueElement (ESDFMap.h:39)
==14181==    by 0x133DAC: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133DAC: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133DAC: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:167)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Address 0x3dea6030 is 1,440 bytes inside a block of size 1,536 free'd
==14181==    at 0x4C3123B: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B428: deallocate (new_allocator.h:125)
==14181==    by 0x12B428: deallocate (alloc_traits.h:462)
==14181==    by 0x12B428: _M_deallocate (stl_vector.h:180)
==14181==    by 0x12B428: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:448)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181==  Block was alloc'd at
==14181==    at 0x4C3017F: operator new(unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==14181==    by 0x12B353: allocate (new_allocator.h:111)
==14181==    by 0x12B353: allocate (alloc_traits.h:436)
==14181==    by 0x12B353: _M_allocate (stl_vector.h:172)
==14181==    by 0x12B353: void std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(__gnu_cxx::__normal_iterator<Eigen::Matrix<double, 3, 1, 0, 3, 1>*, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<double, 3, 1, 0, 3, 1>&&) (vector.tcc:406)
==14181==    by 0x12B24A: emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (vector.tcc:105)
==14181==    by 0x12B24A: push_back (stl_vector.h:954)
==14181==    by 0x12B24A: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:121)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)
==14181== 

valgrind: m_mallocfree.c:307 (get_bszB_as_is): Assertion 'bszB_lo == bszB_hi' failed.
valgrind: Heap block lo/hi size mismatch: lo = 1600, hi = 69.
This is probably caused by your program erroneously writing past the
end of a heap block and corrupting heap metadata.  If you fix any
invalid writes reported by Memcheck, this assertion failure will
probably go away.  Please try that before reporting this as a bug.


host stacktrace:
==14181==    at 0x5804417A: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x58044294: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x58044419: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x58052F80: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x5803CE8A: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x5803B593: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x5803FC5D: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x5803A88B: ??? (in /usr/lib/valgrind/memcheck-amd64-linux)
==14181==    by 0x1003A1337A: ???
==14181==    by 0x101378AF1F: ???
==14181==    by 0x1189F: ???
==14181==    by 0x1002017F9F: ???
==14181==    by 0x101378AF07: ???
==14181==    by 0x101378AF1F: ???
==14181==    by 0xC00: ???
==14181==    by 0xBFF: ???
==14181==    by 0x160C1: ???

sched status:
  running_tid=10

Thread 1: status = VgTs_WaitSys (lwpid 14181)
==14181==    at 0x5E8085E: futex_wake (futex-internal.h:231)
==14181==    by 0x5E8085E: pthread_cond_broadcast@@GLIBC_2.3.2 (pthread_cond_broadcast.c:86)
==14181==    by 0x4F6BE18: ros::ROSOutAppender::log(ros::console::levels::Level, char const*, char const*, char const*, int) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x869A4A9: ros::console::impl::Log4cxxAppender::append(log4cxx::helpers::ObjectPtrT<log4cxx::spi::LoggingEvent> const&, log4cxx::helpers::Pool&) (in /opt/ros/melodic/lib/librosconsole_log4cxx.so)
==14181==    by 0x8B71664: log4cxx::AppenderSkeleton::doAppend(log4cxx::helpers::ObjectPtrT<log4cxx::spi::LoggingEvent> const&, log4cxx::helpers::Pool&) (in /usr/lib/x86_64-linux-gnu/liblog4cxx.so.10.0.0)
==14181==    by 0x8B6F60E: log4cxx::helpers::AppenderAttachableImpl::appendLoopOnAppenders(log4cxx::helpers::ObjectPtrT<log4cxx::spi::LoggingEvent> const&, log4cxx::helpers::Pool&) (in /usr/lib/x86_64-linux-gnu/liblog4cxx.so.10.0.0)
==14181==    by 0x8BB4ADC: log4cxx::Logger::callAppenders(log4cxx::helpers::ObjectPtrT<log4cxx::spi::LoggingEvent> const&, log4cxx::helpers::Pool&) const (in /usr/lib/x86_64-linux-gnu/liblog4cxx.so.10.0.0)
==14181==    by 0x8BB4D0D: log4cxx::Logger::forcedLog(log4cxx::helpers::ObjectPtrT<log4cxx::Level> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, log4cxx::spi::LocationInfo const&) const (in /usr/lib/x86_64-linux-gnu/liblog4cxx.so.10.0.0)
==14181==    by 0x8696EAA: ros::console::impl::print(void*, ros::console::levels::Level, char const*, char const*, char const*, int) (in /opt/ros/melodic/lib/librosconsole_log4cxx.so)
==14181==    by 0x5631A20: ros::console::print(ros::console::FilterBase*, void*, ros::console::levels::Level, std::__cxx11::basic_stringstream<char, std::char_traits<char>, std::allocator<char> > const&, char const*, int, char const*) (in /opt/ros/melodic/lib/librosconsole.so)
==14181==    by 0x13E1BF: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastMultithread() (Fiesta.h:297)
==14181==    by 0x14573A: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::SynchronizationAndProcess() (Fiesta.h:437)
==14181==    by 0x13BAAD: operator() (function_template.hpp:759)
==14181==    by 0x13BAAD: boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>) (function_template.hpp:159)
==14181==    by 0x145E9E: operator() (function_template.hpp:759)
==14181==    by 0x145E9E: ros::SubscriptionCallbackHelperT<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) (subscription_callback_helper.h:144)
==14181==    by 0x4F81791: ros::SubscriptionQueue::call() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F2BE8B: ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F2D27A: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F852E8: ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F6DB5A: ros::spin() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x12575A: main (test_fiesta.cpp:7)

Thread 2: status = VgTs_WaitSys (lwpid 14222)
==14181==    at 0x5E83384: read (read.c:27)
==14181==    by 0x4F8A22A: ros::PollSet::onLocalPipeEvents(int) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F8CCFA: ros::PollSet::update(int) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F179A4: ros::PollManager::threadFunc() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x79F6BCC: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 3: status = VgTs_WaitSys (lwpid 14223)
==14181==    at 0x70DBBF9: poll (poll.c:29)
==14181==    by 0x75C7EB7: XmlRpc::XmlRpcDispatch::work(double) (in /opt/ros/melodic/lib/libxmlrpcpp.so)
==14181==    by 0x4EFAEE7: ros::XMLRPCManager::serverThreadFunc() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x79F6BCC: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 4: status = VgTs_WaitSys (lwpid 14224)
==14181==    at 0x5E8310D: __lll_lock_wait (lowlevellock.S:135)
==14181==    by 0x5E7DF01: __pthread_mutex_cond_lock (pthread_mutex_lock.c:78)
==14181==    by 0x5E7F8BE: __pthread_cond_wait_common (pthread_cond_wait.c:645)
==14181==    by 0x5E7F8BE: pthread_cond_wait@@GLIBC_2.3.2 (pthread_cond_wait.c:655)
==14181==    by 0x4F6AFCE: ros::ROSOutAppender::logThread() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x79F6BCC: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 5: status = VgTs_WaitSys (lwpid 14225)
==14181==    at 0x5E7FED9: futex_reltimed_wait_cancelable (futex-internal.h:142)
==14181==    by 0x5E7FED9: __pthread_cond_wait_common (pthread_cond_wait.c:533)
==14181==    by 0x5E7FED9: pthread_cond_timedwait@@GLIBC_2.3.2 (pthread_cond_wait.c:667)
==14181==    by 0x4F2D583: ros::CallbackQueue::callAvailable(ros::WallDuration) (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x4F6D6D0: ros::internalCallbackQueueThreadFunc() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x79F6BCC: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 6: status = VgTs_WaitSys (lwpid 14227)
==14181==    at 0x5E7FF85: futex_abstimed_wait_cancelable (futex-internal.h:205)
==14181==    by 0x5E7FF85: __pthread_cond_wait_common (pthread_cond_wait.c:539)
==14181==    by 0x5E7FF85: pthread_cond_timedwait@@GLIBC_2.3.2 (pthread_cond_wait.c:667)
==14181==    by 0x4F05D65: ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::threadFunc() (in /opt/ros/melodic/lib/libroscpp.so)
==14181==    by 0x79F6BCC: ??? (in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 7: status = VgTs_Yielding (lwpid 14883)
==14181==    at 0x12AF7F: Raycast(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >*) (raycast.cpp:118)
==14181==    by 0x13605D: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:233)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 8: status = VgTs_Yielding (lwpid 14884)
==14181==    at 0x12B9B0: fiesta::ESDFMap::Vox2Idx(Eigen::Matrix<int, 3, 1, 0, 3, 1>) (ESDFMap.cpp:89)
==14181==    by 0x12FD3D: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:418)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 10: status = VgTs_Runnable (lwpid 14888)
==14181==    at 0x133E0E: DenseStorage (DenseStorage.h:194)
==14181==    by 0x133E0E: PlainObjectBase (PlainObjectBase.h:504)
==14181==    by 0x133E0E: Matrix (Matrix.h:274)
==14181==    by 0x133E0E: QueueElement (ESDFMap.h:39)
==14181==    by 0x133E0E: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (new_allocator.h:136)
==14181==    by 0x133E0E: construct<fiesta::ESDFMap::QueueElement, fiesta::ESDFMap::QueueElement> (alloc_traits.h:475)
==14181==    by 0x133E0E: _M_push_back_aux<fiesta::ESDFMap::QueueElement> (deque.tcc:491)
==14181==    by 0x133E0E: fiesta::ESDFMap::QueueElement& std::deque<fiesta::ESDFMap::QueueElement, std::allocator<fiesta::ESDFMap::QueueElement> >::emplace_back<fiesta::ESDFMap::QueueElement>(fiesta::ESDFMap::QueueElement&&) (deque.tcc:173)
==14181==    by 0x12FDD5: push_back (stl_deque.h:1558)
==14181==    by 0x12FDD5: push (stl_queue.h:257)
==14181==    by 0x12FDD5: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<int, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:432)
==14181==    by 0x12FF29: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:414)
==14181==    by 0x1360EB: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:248)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)

Thread 11: status = VgTs_Yielding (lwpid 14889)
==14181==    at 0x12B82E: fiesta::ESDFMap::Pos2Vox(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<int, 3, 1, 0, 3, 1>&) (ESDFMap.cpp:76)
==14181==    by 0x12FF0A: fiesta::ESDFMap::SetOccupancy(Eigen::Matrix<double, 3, 1, 0, 3, 1>, int) (ESDFMap.cpp:413)
==14181==    by 0x135F7F: fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess(int, int, int) (Fiesta.h:213)
==14181==    by 0x6AE366E: ??? (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==14181==    by 0x5E796DA: start_thread (pthread_create.c:463)
==14181==    by 0x70E888E: clone (clone.S:95)


Note: see also the FAQ in the source distribution.
It contains workarounds to several common problems.
In particular, if Valgrind aborted or crashed after
identifying problems in your program, there's a good chance
that fixing those problems will prevent Valgrind aborting or
crashing, especially if it happened in m_mallocfree.c.

If that doesn't help, please report this bug to: www.valgrind.org

In the bug report, send all the above text, the valgrind
version, and what OS and version you are using.  Thanks.

================================================================================REQUIRED process [fiesta-2] has died!
process has died [pid 14181, exit code 1, cmd valgrind /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/412f2c60-4c93-11ea-a841-00d8617c19f3/fiesta-2.log].
log file: /home/seunghwan/.ros/log/412f2c60-4c93-11ea-a841-00d8617c19f3/fiesta-2*.log
Initiating shutdown!
================================================================================

gdb trace differs if try to run 24 threads

Pointcloud Size:	307200
[ WARN] [1581390896.603124842]: part : 12800 thread count : 0
[New Thread 0x7fffed31b700 (LWP 19080)]
[ WARN] [1581390896.604619455]: part : 12800 thread count : 1
[New Thread 0x7fffecb1a700 (LWP 19081)]
[Thread 0x7fffed31b700 (LWP 19080) exited]
[ WARN] [1581390896.604806973]: part : 12800 thread count : 2
[New Thread 0x7fffe4f2a700 (LWP 19082)]
[ WARN] [1581390896.604949472]: part : 12800 thread count : 3
[New Thread 0x7fffc4886700 (LWP 19083)]
[ WARN] [1581390896.605085740]: part : 12800 thread count : 4
[New Thread 0x7fffb3cab700 (LWP 19084)]

Thread 9 "test_fiesta" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffe4f2a700 (LWP 19082)]
0x00007ffff58f0cc1 in _int_free (have_lock=0, p=<optimized out>, av=0x7fff9c000020) at malloc.c:4295
4295	malloc.c: No such file or directory.
(gdb) bt
#0  0x00007ffff58f0cc1 in _int_free (have_lock=0, p=<optimized out>, av=0x7fff9c000020) at malloc.c:4295
#1  __GI___libc_free (mem=0x7fff9c0025f0) at malloc.c:3124
#2  0x0000555555577429 in __gnu_cxx::new_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> >::deallocate (this=<optimized out>, __p=<optimized out>)
    at /usr/include/c++/7/ext/new_allocator.h:125
#3  std::allocator_traits<std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::deallocate (__a=..., __n=<optimized out>, __p=<optimized out>)
    at /usr/include/c++/7/bits/alloc_traits.h:462
#4  std::_Vector_base<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_deallocate (this=<optimized out>, 
    __n=<optimized out>, __p=<optimized out>) at /usr/include/c++/7/bits/stl_vector.h:180
#5  std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::_M_realloc_insert<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (this=0x7fffe4f29cc0, __position=
          {<Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >> = {<Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >> = {<Eigen::DenseBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 3>> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 1>> = {<Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 0>> = {<Eigen::EigenBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >> = {<No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, <No data fields>}, m_storage = {m_data = {array = {5.1988896890259714e-312, 2.9149873104633546e-322, 0}}}}, <No data fields>}, __args#0=...) at /usr/include/c++/7/bits/vector.tcc:448
#6  0x000055555557724b in std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::emplace_back<Eigen::Matrix<double, 3, 1, 0, 3, 1> > (this=0x7fffe4f29cc0) at /usr/include/c++/7/bits/vector.tcc:105
#7  std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >::push_back (__x=..., this=0x7fffe4f29cc0)
    at /usr/include/c++/7/bits/stl_vector.h:954
#8  Raycast (start=..., end=..., min=..., max=..., output=0x7fffe4f29cc0) at /home/seunghwan/fast_ws/src/FIESTA/src/raycast.cpp:121
#9  0x000055555558205e in fiesta::Fiesta<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>, boost::shared_ptr<geometry_msgs::TransformStamped_<std::allocator<void> > const> >::RaycastProcess (this=0x7fffffff9f90, i=<optimized out>, part=<optimized out>, tt=1)
    at /home/seunghwan/fast_ws/src/FIESTA/include/Fiesta.h:233
#10 0x00007ffff5f1f66f in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#11 0x00007ffff6b876db in start_thread (arg=0x7fffe4f2a700) at pthread_create.c:463
#12 0x00007ffff597a88f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

[minhdov]

I encountered the same issue. Any updates?