[registration] ndt->setInputTarget() running error

[kitzhongzhengqi]

Hello,when I use ndt or ndt_omp as below:

    pcl::PointCloud<RsPointXYZIRT>::Ptr map_now(new pcl::PointCloud<RsPointXYZIRT>); // 创建点云（指针）
        for (const Area &area : cachedAreas)
        {
            *map_now += *area.cloud_points;
        }

        rwLock_map_points.writeLock();
        map_nums = map_now->width * map_now->height;
        rwLock_map_points.writeUnlock();
            map_now->is_dense = true;
            map_now->width = map_nums;
            map_now->height=1;
            map_now->resize(map_nums);
        rwLock_map_points.readLock();
        std::cout << GREEN  << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Loaded "
                  << map_now->width * map_now->height
                  << " data points from pcd files"
                  << std::endl;
        rwLock_map_points.readUnlock();
        mpmapout.cloud_ptr.reset(new pcl::PointCloud<RsPointXYZIRT>());
        mpmapout.cloud_ptr = map_now;
        
        pcl::PointCloud<RsPointXYZIRT>::Ptr cloud_map(new pcl::PointCloud<RsPointXYZIRT>);
   

        for (size_t i = 0; i < map_now->size(); i++)
          // for(size_t i = cloud_.size()-1; i >= 0; i--)
          {
            if(isnan(map_now->points[i].x) or isnan(map_now->points[i].y) or isnan(map_now->points[i].z) ){
                continue;
            }
            const auto &pt = map_now->at(i);
            cloud_map->points.push_back(pt);
          }
    


        // checkMapCompleteness(map_now, now_points);
        // cout<<RED<<"lock"<<endl;
        ndt_map_mtx_.lock();
        if (use_ndtomp)
        {
            // ndtomp_map_->setInputTarget(map_now);
            ndtomp_->setInputTarget(cloud_map);
        }
        else
        {
            ndt_map_->setInputTarget(cloud_map);
        }

I got a running error :

terminate called after throwing an instance of 'std::bad_array_new_length'
  what():  std::bad_array_new_length
I0517 14:52:42.728947  5706 localization_adapter_node.cpp:1742] ++++++++++++front: 1683708577.604877 1684306362.705401 1684306362.728936
I0517 14:52:42.728989  5706 localization_adapter_node.cpp:1744] front arrive out of range
I0517 14:52:42.740120  5731 lidar_l10n.cpp:9613] Published lidar status: 0x1152101,0x1152103,0x115210B,0x115210C,0x115210D,0x115210E,0x115240A
E0517 14:52:42.740662  5732 localization_adapter_node.cpp:1171] Published fusion err code: 0x1151105,0x1151106,0x1151501;  Fusion msg: ERROR - MISSING_INPUTS !

Thread 23 "localization_ad" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fff9f7fe700 (LWP 5734)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff42d3859 in __GI_abort () at abort.c:79
#2  0x00007ffff46ce911 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff46da38c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff46da3f7 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff46da6a9 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff46ce3e6 in __cxa_throw_bad_array_new_length () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff65828bf in pcl::KdTreeFLANN<apal_sensor_service::RsPointXYZIRT, flann::L2_Simple<float> >::convertCloudToArray (this=0x7fffb409e860, cloud=...)
    at /usr/include/pcl-1.10/pcl/kdtree/impl/kdtree_flann.hpp:238
#8  0x00007ffff6573c1a in pcl::KdTreeFLANN<apal_sensor_service::RsPointXYZIRT, flann::L2_Simple<float> >::setInputCloud (this=0x7fffb409e860, cloud=..., indices=...)
    at /usr/include/pcl-1.10/pcl/kdtree/impl/kdtree_flann.hpp:116
#9  0x00007ffff675e41b in pclomp::NormalDistributionsTransform<apal_sensor_service::RsPointXYZIRT, apal_sensor_service::RsPointXYZIRT>::setInputTarget(boost::shared_ptr<pcl::PointCloud<apal_sensor_service::RsPointXYZIRT> const> const&) () from /home/apal-robo-percp/colcon_ws/src/ros2_loc_node/apal_localization/adapter/install/ros2_loc_node/lib/liblocalization_adapter.so
#10 0x00007ffff6531e83 in LidarL10n::DynamicLoadMap (this=0x7fffb4009060) at /home/apal-robo-percp/colcon_ws/src/ros2_loc_node/apal_localization/src/l10n/src/lidar_l10n/lidar_l10n.cpp:1847
#11 0x00007ffff63e9c4b in apal::localization::Localization::loadmap (this=0x7fffb4001eb0)
    at /home/apal-robo-percp/colcon_ws/src/ros2_loc_node/apal_localization/src/interface/src/localization_impl.cpp:287
#12 0x0000555555ac53b3 in std::__invoke_impl<void, void (apal::localization::LocalizationInterface::*)(), std::shared_ptr<apal::localization::LocalizationInterface>>(std::__invoke_memfun_deref, void (apal::localization::LocalizationInterface::*&&)(), std::shared_ptr<apal::localization::LocalizationInterface>&&) (__f=@0x555556153518: &virtual table offset 328, __t=...)
    at /usr/include/c++/9/bits/invoke.h:73
#13 0x0000555555aab98d in std::__invoke<void (apal::localization::LocalizationInterface::*)(), std::shared_ptr<apal::localization::LocalizationInterface> > (
    __fn=@0x555556153518: &virtual table offset 328) at /usr/include/c++/9/bits/invoke.h:95
#14 0x0000555555a8a431 in std::thread::_Invoker<std::tuple<void (apal::localization::LocalizationInterface::*)(), std::shared_ptr<apal::localization::LocalizationInterface> > >::_M_invoke<0ul, 1ul> (
    this=0x555556153508) at /usr/include/c++/9/thread:244
#15 0x0000555555a09874 in std::thread::_Invoker<std::tuple<void (apal::localization::LocalizationInterface::*)(), std::shared_ptr<apal::localization::LocalizationInterface> > >::operator() (
    this=0x555556153508) at /usr/include/c++/9/thread:251
#16 0x00005555559ee028 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<void (apal::localization::LocalizationInterface::*)(), std::shared_ptr<apal::localization::LocalizationInterface> > > >::_M_run (this=0x555556153500) at /usr/include/c++/9/thread:195
#17 0x00007ffff4706de4 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#18 0x00007ffff44ab609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#19 0x00007ffff43d0133 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

@mvieth thanks for your help!

[mvieth]

@kitzhongzhengqi In your gdb backtrace, I see pclomp::NormalDistributionsTransform. Does the same error also happen if you use the ndt from PCL? If yes, please post the backtrace from that instead.
What is printed if you add std::cout << "cloud_map->size()=" << cloud_map->size() << std::endl; shortly before setInputTarget?

[kitzhongzhengqi]

@mvieth with official ndt the bug doesn't occur, do you have some suggestions for this bug ,thanks a lot!

[mvieth]

@kitzhongzhengqi As I said in the other issue, ndt_omp (pclomp::NormalDistributionsTransform) is not an official part of PCL. If the error does not occur when you use the ndt from PCL, then the problem is very likely inside ndt_omp, and you should ask there.