Trying to run with my own stereo setup

EhrazImam commented 1 year ago

Hi i am trying to run it with my own stereo pair, And i am not able to run it.

roslaunch ov_msckf subscribe.launch

process[ov_msckf-1]: started with pid [17023]
overriding node verbosity with value from ROS!
Setting printing level to: INFO
overriding node max_cameras with value from ROS!
the node gyroscope_noise_density of type [d] was not found...
the node gyroscope_random_walk of type [d] was not found...
the node accelerometer_noise_density of type [d] was not found...
the node accelerometer_random_walk of type [d] was not found...
overriding node max_cameras with value from ROS!
overriding node use_stereo with value from ROS!
overriding node record_timing_information with value from ROS!
overriding node record_timing_filepath with value from ROS!
the node gyroscope_noise_density of type [d] was not found...
the node gyroscope_random_walk of type [d] was not found...
the node accelerometer_noise_density of type [d] was not found...
the node accelerometer_random_walk of type [d] was not found...
overriding node max_cameras with value from ROS!
overriding node use_stereo with value from ROS!
the node rostopic of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
**subscribing to IMU: /imu0**
subscribing to cam (stereo): /video0
subscribing to cam (stereo): /video1
unable to parse all parameters, please fix
terminate called after throwing an instance of 'boost::wrapexcept<boost::lock_error>'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
================================================================================REQUIRED process [ov_msckf-1] has died!
process has died [pid 17023, exit code -6, cmd /mnt/SSD/catkin_ws/devel/lib/ov_msckf/run_subscribe_msckf __name:=ov_msckf __log:=/home/enord/.ros/log/f54ca916-85e3-11ed-a7b2-ec2e98ca6411/ov_msckf-1.log].
log file: /home/enord/.ros/log/f54ca916-85e3-11ed-a7b2-ec2e98ca6411/ov_msckf-1*.log
Initiating shutdown!
================================================================================
[ov_msckf-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Here is my launch file and yaml file

<launch>

    <!-- what config we are going to run (should match folder name) -->
    <arg name="verbosity"   default="INFO" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
    <arg name="config"      default="econ" /> <!-- euroc_mav, tum_vi, rpng_aruco -->
    <arg name="config_path" default="$(find ov_msckf)/../config/$(arg config)/econ.yaml" />

    <!-- mono or stereo and what ros bag to play -->
    <arg name="max_cameras" default="2" />
    <arg name="use_stereo"  default="true" />

    <!-- saving trajectory path and timing information -->
    <arg name="dosave"      default="false" />
    <arg name="dotime"      default="false" />
    <arg name="path_est"    default="/tmp/traj_estimate.txt" />
    <arg name="path_time"   default="/tmp/traj_timing.txt" />

    <!-- if we should viz the groundtruth -->
    <arg name="dolivetraj"  default="false" />

    <!-- MASTER NODE! -->
<!--    <node name="ov_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
    <node name="ov_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">

        <!-- master configuration object -->
        <param name="verbosity"              type="string" value="$(arg verbosity)" />
        <param name="config_path"            type="string" value="$(arg config_path)" />

        <!-- world/filter parameters -->
        <param name="use_stereo"             type="bool"   value="$(arg use_stereo)" />
        <param name="max_cameras"            type="int"    value="$(arg max_cameras)" />

        <!-- timing statistics recording -->
        <param name="record_timing_information"   type="bool"   value="$(arg dotime)" />
        <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />

    </node>

    <!-- record the trajectory if enabled -->
    <group if="$(arg dosave)">
        <node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
            <param name="topic"      type="str" value="/ov_msckf/poseimu" />
            <param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
            <param name="output"     type="str" value="$(arg path_est)" />
        </node>
    </group>

    <!-- path viz of aligned gt -->
    <group if="$(arg dolivetraj)">
        <node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
            <param name="alignment_type" type="str" value="posyaw" />
            <param name="path_gt"        type="str" value="$(arg path_gt)" />
        </node>
    </group>

</launch>

%YAML:1.0 # need to specify the file type at the top!

cam0:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [-0.00985289, -0.10295624, 0.99463709, 0.00901709]
    - [-0.99993869, 0.00604112, -0.00928008, 0.01079419]
    - [-0.00505328, -0.99466754, -0.10300945, -0.0001016]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.31611411793190636, 0.0859737553368236, 0.0006737650935018986, 0.0008035703899149116]
  distortion_model: radtan
  intrinsics: [393.96486960957077, 417.80063692284233, 372.72636823475375, 222.78334066921914]
  resolution: [720, 480]
  rostopic: /video0

cam1:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [-0.0074891, -0.1062204, 0.99431441, 0.00932111]
    - [-0.99992894, 0.01001879, -0.0064611, -0.0988453]
    - [-0.00927552, -0.99429214, -0.10628788, -0.00103599]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.31965731106092976, 0.09150401559934115, -2.7714333658321444e-05, -0.002391938689798196]
  distortion_model: radtan
  intrinsics: [392.2215854310774, 415.85215678071023, 368.6123974928111, 218.38944892838222]
  resolution: [720, 480]
  rostopic: /video1

%YAML:1.0

imu0:
  T_i_b:
    - [1.0, 0.0, 0.0, 0.0]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 1.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
accelerometer_noise_density: 0.03624460410838351
accelerometer_random_walk: 0.001986100134698512
gyroscope_noise_density: 0.003482312588398656
gyroscope_random_walk: 8.281246049118099e-05
model: calibrated
rostopic: /mavros/imu/data
time_offset: 0.0
update_rate: 187.0

I dont know why it is subscribing to IMU: /imu0 as i have changed it but it is taking by default /imu0

WoosikLee2510 commented 1 year ago

the node gyroscope_noise_density of type [d] was not found...
the node gyroscope_random_walk of type [d] was not found...
the node accelerometer_noise_density of type [d] was not found...
the node accelerometer_random_walk of type [d] was not found...

This is THE problem. Do you have the config for IMU in the same directory of config for the estimator and the camera? You can set these values in the roslaunch file if not available.

EhrazImam commented 1 year ago

@WoosikLee2510 Thankyou, I am able to run it but when i am running it in real world forward is x- and backward is x+. Can you you help me what i am doing wrong and how can i change the orientation where X should be forward and X- should be backward. As i have tried the same Calibration data in VINS-Fusion where the orientation is correct.

WoosikLee2510 commented 1 year ago

Not clear what you mean by saying real-world forward is x- and backward is x+. Can you elaborate in detail?

EhrazImam commented 1 year ago

@WoosikLee2510 When i am moving my camera forward the value is increasing in -X(negative) coordinate while Backward in +X(positive). The orientation is reversed .

goldbattle commented 1 year ago

Visual-inertial systems have arbitrary yaw. Additionally, depending on the sign of gravity used the by the IMU driver you might have a system which is "upside down". I recommend investigating along these directions for your system.

EhrazImam commented 1 year ago

Hi @goldbattle I am using Pixhawk imu and secondly i have tried it in vins fusion and it worked well with the correct orientation.

goldbattle commented 1 year ago

Can you try commenting this in to see if this fixes your problem? https://github.com/rpng/open_vins/blob/master/ov_init/src/utils/helper.h#L161-L165

EhrazImam commented 1 year ago

Hi @goldbattle they are already commented.

goldbattle commented 1 year ago

Yes, uncomment them.

On Fri, Jan 6, 2023 at 2:11 AM EhrazImam @.***> wrote:

Hi @goldbattle https://github.com/goldbattle they are already commented.

— Reply to this email directly, view it on GitHub https://github.com/rpng/open_vins/issues/297#issuecomment-1373231190, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYQADUJKFOJLFKQTLLLWQ7AR7ANCNFSM6AAAAAATKNNHXQ . You are receiving this because you were mentioned.Message ID: @.***>

EhrazImam commented 1 year ago

@goldbattle Thankyou it worked for me. Is this supported with rolling shutter camera, and where are the changes you made it to work it for rolling shutter.

goldbattle commented 1 year ago

I would refer you to some works I have on my website on it as you would need to implement this yourself:

MIMC-VINS: A Versatile and Resilient Multi-IMU Multi-Camera Visual-Inertial Navigation System - Link
Rolling Shutter Camera Calibration - Link
Continuous-time batch trajectory estimation using temporal basis functions - Link
Vision-aided inertial navigation with rolling-shutter cameras - Link
Efficient Visual-Inertial Navigation using a Rolling-Shutter Camera with Inaccurate Timestamps - Link
Real-time Motion Tracking on a Cellphone using Inertial Sensing and a Rolling-Shutter Camera - Link
3-D Motion Estimation and Online Temporal Calibration for Camera-IMU Systems - Link
High-fidelity Sensor Modeling and Self-Calibration in Vision-aided Inertial Navigation - Link

goldbattle commented 1 year ago

Opps, I will keep this open till I fix it on the master branch.

EhrazImam commented 1 year ago

Hi @goldbattle thankyou for the changes i am able to run it but after running it for several time on my hardware i can noticed 2/3 time it's stopped working.

error

EhrazImam commented 1 year ago

@goldbattle @WoosikLee2510 @NikolausDemmel please look into this as it is happening again and agian

goldbattle commented 1 year ago

@EhrazImam Please create a new issue with the problem along with the dataset it is failing on.

rpng / open_vins

Trying to run with my own stereo setup #297