Stereo Fisheye axis.squaredNorm()

[gftabor]

I am working with 2 fisheye cameras and trying to run stereo SVO. SVO crashes after only a few seconds with the below error. The issue does not necessarily happen at the same time in my bag file.

F0204 16:55:15.496071  5228 angle-axis-inl.h:112] Check failed: (axis.squaredNorm()) <= (static_cast<Scalar>(1.0))+(static_cast<Scalar>(1e-4)) (-nan vs. 1.0001) 
*** Check failure stack trace: ***
    @     0x7f243965b304  google::LogMessage::Fail()
    @     0x7f243965b248  google::LogMessage::SendToLog()
    @     0x7f243965ab84  google::LogMessage::Flush()
    @     0x7f243965e6ab  google::LogMessageFatal::~LogMessageFatal()
    @     0x7f243e51c9f0  kindr::minimal::AngleAxisTemplate<>::setAxis()
    @     0x7f243e51a93c  svo::Matcher::scanEpipolarUnitSphere()
    @     0x7f243e51bfff  svo::Matcher::findEpipolarMatchDirect()
    @     0x7f243e521106  svo::depth_filter_utils::updateSeed()
    @     0x7f243e52463a  svo::DepthFilter::updateSeeds()
    @     0x7f243e9ad970  svo::FrameHandlerStereo::makeKeyframe()
    @     0x7f243e9b2da6  svo::FrameHandlerStereo::processFrame()
    @     0x7f243e9c7d78  svo::FrameHandlerBase::addFrameBundle()
    @     0x7f243e9c8d50  svo::FrameHandlerBase::addImageBundle()
    @     0x7f2440e52c1c  svo::SvoInterface::stereoCallback()
    @     0x7f2440e5ed98  boost::detail::function::void_function_obj_invoker9<>::invoke()
    @     0x7f2440e6dab9  boost::function9<>::operator()()
    @     0x7f2440e6e169  message_filters::CallbackHelper9T<>::call()
    @     0x7f2440e683b8  message_filters::sync_policies::ExactTime<>::checkTuple()
    @     0x7f2440e6a669  message_filters::Synchronizer<>::cb<>()
    @     0x7f2440e67722  image_transport::SubscriberFilter::cb()
    @     0x7f2428ceba89  image_transport::RawSubscriber::internalCallback()
    @     0x7f243ec8ca7c  boost::detail::function::void_function_obj_invoker1<>::invoke()
    @     0x7f2428cf3613  ros::SubscriptionCallbackHelperT<>::call()
    @     0x7f24411eb44d  ros::SubscriptionQueue::call()
    @     0x7f2441191210  ros::CallbackQueue::callOneCB()
    @     0x7f2441192683  ros::CallbackQueue::callAvailable()
    @     0x7f2440e558db  svo::SvoInterface::stereoLoop()
    @     0x7f2440b57510  (unknown)
    @     0x7f243f3f06ba  start_thread
    @     0x7f24405c141d  clone
    @              (nil)  (unknown)

Here is the bag file I recorded and have been using for debugging https://drive.google.com/file/d/19wk18eWXSzaSYfriFq7O1hxZRlqIbhti/view?usp=sharing

Here is a video of RVIZ running with the bag file https://www.youtube.com/watch?v=haNSmDuULG4

From the launch file

    <node pkg="svo_ros" type="svo_node" name="svo" clear_params="true" output="screen" required="true">
    <!-- Camera and IMU topics to subscribe to -->
    <param name="cam1_topic" value="/left/image_raw" type="str" />
    <param name="cam0_topic" value="/right/image_raw" type="str" />
    <param name="calib_file" value="$(find svo_ros)/calib/fisheyeTry2.yaml" type="str" />
    <rosparam file="$(find svo_ros)/param/stereo_fisheye.yaml" />
  </node>

The parameters, calibration, and masks are all here https://github.com/asgard-mqp/rpg_svo_example

The parameters in particular, I had to hack together euroc_stereo and fisheye so help there would be appreciated.

https://github.com/asgard-mqp/rpg_svo_example/blob/master/svo_ros/param/stereo_fisheye.yaml

[johnsongash]

Maybe problem with axis?? Check quaternion of initial rotation: T_world_imuinit/qx: 1 T_world_imuinit/qy: 0 T_world_imuinit/qz: 0 T_world_imuinit/qw: 0

[gftabor]

My param yaml sets the initial rotation, removing it has same results.(albeit rotated)

UPDATE: I am able to stop it from crashing by setting scan_epi_unit_sphere to false of course. Fisheye cameras require it to be true so the results are of course poor. I would guess something about dual fisheye causes an unexpected bug. I believe I will need an official response on how to solve my issue. @zhangzichao

[zhangzichao]

Thanks for reporting the problem, actually we have not tested on a dual fisheye setup before, might be some potential bug. I will try to have a look at the bag.

[gftabor]

I would be happy to provide anything else that would be helpful with debugging. I guess I could add (but I really doubt it matters) I have had the issue consistently with Odroid XU4 running 14.04 and laptop running 16.04.

[gftabor]

@zhangzichao have you had an opportunity to look into the bag file.

[bednarhonza]

@zhangzichao @gftabor Have you solved this issue? I'm facing the same problem.

Thank you for your help! Jan