tinkerforge imu

[consoante]

http://answers.ros.org/question/242417/sensor-fusion-of-imu-and-asus-for-rtab-map-with-robot_localization/
https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/sensor_fusion_kinect_brick.launch

In the quoted topic and launch files the tinkerforge imu is being used but i am not able to find the  imu_brick package. Is it public?

I have this imu and would like to give it a try.

I have the package https://github.com/gus484/ros-tinkerforge_sensors installed and running and can get an output from /tfsensors/imu1 but the result of the robot_localization package is...silence. I don't have odometry only imu and would like to test:

kinect 2 + imu
Kinect2 + laser (already successful)

Any thoughts?

thanks

[consoante]

Making progress...running the devel branch of ros-tinkerforge_sensors

I am running robot localization to filter the imu data, launch file is: http://pastebin.com/mb6abYwf

RtabMap launch file is http://pastebin.com/LNTva9Vu 

Frames and graph seem ok, however the base link / origin suffers from sudden jumps where the map / cloud moves across the yaw, pitch or roll axis randomly and unpredictable.

I understand this is not the approach used in the sensor fusion, given that there is no visual odometry only ekf imu data.

The trajectory appears as a group of nodes...indicating that it is actually not a pose graph?

[matlabbe]

Hi, the imu_brick package is a homemade package. ros-tinkerforge_sensors looks fine.

I'm surprised that you have relatively good results with only IMU as odometry. I'll give a try with your setup.

The trajectory should be a pose graph. Can you show the Graph view? Well, normally there are links between the nodes in the 3D view. Which version are you using?

cheers

[consoante]

Many thanks for the support first of all. I am using version 0.11.10.
And yes i was surprised as well: one thing I did not mention was that the map restarts frequently with IMU only.

Graph view is something like:


I seem to have a working EKF IMU + VO setup using the following launch.
However, drift is immediately visible even in zero velocity. As soon as movement begins, the cloud diverges.

Any advice?

//ro

[matlabbe]

Hi,

It is maybe a TF problem. Is kinect2_link in optical frame (x right, y down, z forward) or robot frame (x forward, y left, z up)? You may need an optical rotation transform between /base_link and /kinect2_link if kinect2_link is in optical frame. Make also sure your IMU is x forward, y left and z up. See the static transforms defined in sensor_fusion_kinect_brick.launch and the setup picture in this post. Verify also that your ros package used for IMU provides transforms/angles in robot referential. I copied what I am using here: imu_brick_node.cpp.

You should not have a static transform between /odom and /base_link, it is robot_localization that publishes it.

You may want to disable IMU acceleration estimation from robot_localization, if it is causing too much drift:

<rosparam param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>

cheers

[consoante]

hello,

I corrected the position of the IMU and the rotation of the camera, launch. as per your suggestions. As far as I can tell the node uses radians: code, so the units are apparently ok.

The tree and graph seem ok, but when I echo /odom there is no output, given that robot_localization outputs to /odometry/filtered. I don't know if the filtered transformation is actually being used.

I am also unsure of whether the imu_link is needed here.

The pose seems stable, but the cloud diverges even when stopped.





Thank you!

[matlabbe]

Hi,

Be aware that setting TF /imu_link after the optical rotation, it will rotate the IMU. In comparison to example, your setup should be instead:

<node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf" args="-0.032 0.0 0.032 0.0 0.0 0.0 /base_link /imu_link 20" />

assuming that /base_link is the camera position. Make sure your IMU message has frame_id set to imu_link.

/odom is a TF not a topic:

$rosrun tf tf_echo /odom /base_link

In your setup, rtabmap is subscribing to TF, not the topic. Both approaches work.

cheers

[consoante]

Thanks once more.

Unfortunately your corrections don't improve the results.

echo of the TF with the platform completely stable gives me very strange rotation outputs.  The values for the rotation vary with every update as well.

- Translation: [0.007, -0.004, -0.001]
- Rotation: in Quaternion [-0.052, -0.047, -0.052, 0.996]
            in RPY (radian) [-0.100, -0.099, -0.099]
            in RPY (degree) [-5.702, -5.694, -5.701]

Does this setup work for you?

Cheers

[matlabbe]

In your launch file, only IMU is used for angular estimation. So if odometry is drifting in rotation, compare carefully IMU and odom messages to see if it comes from IMU or ekf_localization.

cheers

[consoante]

Hi,

Sorry but I am not following,  the vo is being fed into the efk_localization node meaning it is accounted for to determine the position of the platform. no?

Thanks,

//ro

[matlabbe]

Hi,

In your launch file, /vo is used to estimate translation while /tfsensors/imu1 is used to estimate rotation. If output of ekf_localization drifts in rotation, it is because of the IMU. If the output drifts in translation, it is because of the /vo. You must then compare the inputs and output of ekf_localization to see if the drift comes from ekf_localization or from the inputs.

<rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, 
                                      true,  true,  true, 
                                     false, false, false, 
                                      true,  true,  true,
                                      false, false, false]</rosparam>

cheers

[yalan2017]

Hi
 im doing almost same work but i have error and warning :
as mentioned in this post  also corrected this part "use /base_link frame as frame_id for visual odometry and rtabmap nodes." but still i have problem.
kinect+_imu
i use sensor_fusion_brick.launch  

 WARN] [1502370721.827261150]: odometry: Could not get transform from odom to camera_rgb_frame (stamp=1502370721.704958) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error="Lookup would require extrapolation into the future.  Requested time 1502370721.704958052 but the latest data is at time 1502370461.914175556, when looking up transform from frame [camera_rgb_frame] to frame [odom]. canTransform returned after 0.101071 timeout was 0.1."
[ERROR] [1502370721.827344111]: "guess_from_tf" is true, but guess cannot be computed between frames "odom" -> "camera_rgb_frame". Aborting odometry update...

thank you

[consoante]

Send me your launch file

[yalan2017]

Hi

I have updated, now don't have last problem  

once i start to mover camera i see this warning!

[ INFO] [1502453646.453363080]: Odom: quality=121, std dev=0.017200m|0.017200rad, update time=0.028877s
[ INFO] [1502453646.575189888]: Odom: quality=130, std dev=0.025488m|0.025488rad, update time=0.029338s
[ INFO] [1502453646.661170989]: Odom: quality=57, std dev=0.033019m|0.033019rad, update time=0.023028s
[ INFO] [1502453646.792336136]: Odom: quality=123, std dev=0.040291m|0.040291rad, update time=0.028611s
[ INFO] [1502453646.891922868]: Odom: quality=91, std dev=0.033665m|0.033665rad, update time=0.027490s
[ WARN] (2017-08-11 20:14:07.015) OdometryF2M.cpp:199::computeTransform() Failed to find a transformation with the provided guess (xyz=0.007887,-0.004309,0.006024 rpy=-0.012528,0.014032,0.000450), trying again without a guess.
[ WARN] (2017-08-11 20:14:07.029) OdometryF2M.cpp:207::computeTransform() Trial with no guess still fail.
[ WARN] (2017-08-11 20:14:07.029) OdometryF2M.cpp:380::computeTransform() Registration failed: "Not enough inliers 12/20 (matches=35) between -1 and 83"
[ WARN] [1502453647.029654717]: Odometry lost! Odometry will be reset after next 1 consecutive unsuccessful odometry updates...
[ WARN] [1502453647.029714002]: Odometry automatically reset to latest odometry pose available from TF (odom->camera_rgb_frame)!
[ INFO] [1502453647.029991933]: Odom: quality=12, std dev=0.000000m|0.000000rad, update time=0.045969s
[ INFO] [1502453647.106836282]: Odom: quality=0, std dev=99.995000m|99.995000rad, update time=0.024315s
[ INFO] [1502453647.236805482]: Odom: quality=73, std dev=0.019294m|0.019294rad, update time=0.037178s



first launch file is rtabmap.launch 
second is sensor_fusion.launch
third one is sensor_fusion_brick_kinect.launch



Thank you

[yalan2017]

Hi
I didn't move my camera around, only rotated it  around X-axis but my graph view shows  something else :




also i have uploaded my IMU code here  .

[ INFO] [1502628026.664002280]: Odom: quality=59, std dev=1.000000m|1.000000rad, update time=0.037198s
[ WARN] (2017-08-13 20:40:26.749) RegistrationVis.cpp:937::computeTransformationImpl() All projected points are outside the camera. Guess (xyz=1646366.625000,-2556339.000000,289085.437500 rpy=-0.124248,-0.035368,-0.011060) is wrong or images are not overlapping.
[ WARN] (2017-08-13 20:40:26.749) RegistrationVis.cpp:1452::computeTransformationImpl() Missing correspondences for registration (-1->6). fromWords = 0 fromImageEmpty=1 toWords = 0 toImageEmpty=0
[ WARN] (2017-08-13 20:40:26.749) OdometryF2M.cpp:199::computeTransform() Failed to find a transformation with the provided guess (xyz=49103.875000,-113817.500000,-16657.250000 rpy=0.000004,0.000033,0.000002), trying again without a guess.
[ WARN] (2017-08-13 20:40:26.763) OdometryF2M.cpp:211::computeTransform() Trial with no guess succeeded.
[ INFO] [1502628026.765015408]: Odom: quality=58, std dev=1.000000m|1.000000rad, update time=0.038664s
^C[ WARN] (2017-08-13 20:40:26.866) RegistrationVis.cpp:937::computeTransformationImpl() All projected points are outside the camera. Guess (xyz=1731672.500000,-2641366.750000,313767.468750 rpy=-0.127404,-0.032928,-0.002147) is wrong or images are not overlapping.
[ WARN] (2017-08-13 20:40:26.866) RegistrationVis.cpp:1452::computeTransformationImpl() Missing correspondences for registration (-1->7). fromWords = 0 fromImageEmpty=1 toWords = 0 toImageEmpty=0
[ WARN] (2017-08-13 20:40:26.866) OdometryF2M.cpp:199::computeTransform() Failed to find a transformation with the provided guess (xyz=64201.500000,-101096.000000,6098.843750 rpy=0.000006,-0.000047,-0.000003), trying again without a guess.


any suggestion for me?
thank you

[matlabbe]

Hi,

Don't use rtabmap.launch. rtabmap and visual odometry (rgbd_odometry) are started in sensor_fusion.launch. Use RVIZ instead of rtabmapviz. Can you show the TF tree?

$ rosrun tf view_frames
$ evince frames.pdf

cheers,
Mathieu

[yalan2017]

Hi
Thank you for your reply and so sorry i reply so late.
now i don't use rtabmap.launch but it seems visual odometry (rgbd_odometry) is different with rtabmap,launch because once in this way i don't have any result in my rviz, after run rviz i see blank blank page ,how ever i show my result here:

1- run  roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
2- run ... sensor_fusion_brick_kinect.launch

and have added my rviz part in sensor fusion launch file





[ WARN] (2017-08-23 18:09:37.328) CoreWrapper.cpp:741::odomUpdate() Odometry is reset (identity pose or high variance (170790777927.134247) detected). Increment map id!
[ WARN] (2017-08-23 18:09:37.328) CoreWrapper.cpp:741::odomUpdate() Odometry is reset (identity pose or high variance (170790777927.134247) detected). Increment map id!
[ WARN] (2017-08-23 18:09:37.328) CoreWrapper.cpp:741::odomUpdate() Odometry is reset (identity pose or high variance (170864381821.955383) detected). Increment map id!
[ WARN] [1503482977.438661398]: odometry: Could not get transform from camera_rgb_frame to kinect2_rgb_optical_frame (stamp=1503482977.309805) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error="Could not find a connection between 'camera_rgb_frame' and 'kinect2_rgb_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.101159 timeout was 0.1."
[ WARN] [1503482977.530552274]: Could not get transform from camera_rgb_frame to kinect2_rgb_optical_frame after 0.200000 seconds (for stamp=1503482977.276557)! Error="Could not find a connection between 'camera_rgb_frame' and 'kinect2_rgb_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.201731 timeout was 0.2.".
[ERROR] [1503482977.530621798]: TF of received image 0 at time 1503482977.276557s is not set!
[ERROR] [1503482977.530661757]: Could not convert rgb/depth msgs! Aborting rtabmap update...
[ WARN] (2017-08-23 18:09:37.537) CoreWrapper.cpp:741::odomUpdate() Odometry is reset (identity pose or high variance (171011724235.140442) detected). Increment map id!
[ WARN] [1503482977.541323387]: odometry: Could not get transform from camera_rgb_frame to kinect2_rgb_optical_frame (stamp=1503482977.409565) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error="Could not find a connection between 'camera_rgb_frame' and 'kinect2_rgb_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.10174 timeout was 0.1."


Thank you for your support

[matlabbe]

Hi,

you are now starting 2 rtabmap nodes, static transform to rotate the kinect2 is published also twice. The frame_id is not correctly set...

Normally, there is no change to be made in sensor_fusion.launch. All modifications would be done in sensor_fusion_kinect_brick.launch. Example with kinect2:

<launch>

  <arg name="localization"      default="false"/>
  <arg name="uid"/>

  <!-- Kinect2: -->
  <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch">
    <arg name="publish_tf" value="true" />
  </include>
  
  <!-- IMU Sensor: -->
  <node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
      <param name="frame_id"                       value="imu_link"/>
      <param name="period_ms"                      value="10"/>
      <param name="uid"              type="string" value="$(arg uid)"/>
      <param name="cov_orientation"  type="double" value="0.0005"/>
      <param name="cov_velocity"     type="double" value="0.00025"/>
      <param name="cov_acceleration" type="double" value="0.1"/>
      <param name="remove_gravitational_acceleration" type="bool" value="true"/>
   </node>
      
   <!-- Rotate the camera -->
   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> 

  <!-- IMU frame: taken from the example, adjust to real position! -->
  <node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
      args="-0.032 0.0 0.032 0.0 0.0 0.0 /kinect2_base_link /imu_link 100" />
      
 
  <include file="$(find rtabmap_ros)/launch/tests/sensor_fusion.launch">
      <arg name="frame_id"               value="kinect2_base_link"/>
      <arg name="localization"           value="$(arg localization)"/>
      <arg name="imu_remove_gravitational_acceleration" value="false"/>

      <arg name="rgb_topic"              value="/kinect2/qhd/image_color_rect" />
      <arg name="depth_topic"            value="/kinect2/qhd/image_depth_rect" />
      <arg name="camera_info_topic"      value="/kinect2/qhd/camera_info" />
  </include>
  
</launch>

[yalan2017]

Hi

I have updated  sensor_fusion_kinect_brick.launch  but still i have such problem as photo:












once i start to move camera i have  this warning :

[ INFO] [1503570156.806258618]: Odom: quality=139, std dev=0.023238m|0.023238rad, update time=0.023501s
[Info] [CudaDepthPacketProcessor] avg. time: 1.82719ms -> ~547.289Hz
[ INFO] [1503570156.926587645]: Odom: quality=126, std dev=0.022799m|0.022799rad, update time=0.027829s
[ INFO] [1503570156.963200598]: rtabmap (102): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0497s, Maps update=0.0007s pub=0.0000s (local map=8, WM=8)
[ INFO] [1503570157.030480269]: Odom: quality=114, std dev=0.025944m|0.025944rad, update time=0.031281s
[ INFO] [1503570157.150678903]: Odom: quality=113, std dev=0.028463m|0.028463rad, update time=0.028098s
[ WARN] (2017-08-24 18:22:37.241) OdometryF2M.cpp:199::computeTransform() Failed to find a transformation with the provided guess (xyz=0.000755,-0.002697,-0.000743 rpy=-0.005086,-0.003363,-0.000308), trying again without a guess.
[ WARN] (2017-08-24 18:22:37.251) OdometryF2M.cpp:207::computeTransform() Trial with no guess still fail.
[ WARN] (2017-08-24 18:22:37.251) OdometryF2M.cpp:380::computeTransform() Registration failed: "Not enough inliers 15/20 (matches=41) between -1 and 817"
[ WARN] [1503570157.252173432]: Odometry lost! Odometry will be reset after next 1 consecutive unsuccessful odometry updates...
[ WARN] [1503570157.252232164]: Odometry automatically reset to latest odometry pose available from TF (odom->kinect2_base_link)!
[ INFO] [1503570157.252458728]: Odom: quality=15, std dev=0.000000m|0.000000rad, update time=0.036078s
[ INFO] [1503570157.355299915]: Odom: quality=0, std dev=99.995000m|99.995000rad, update time=0.020238s
[ INFO] [1503570157.462053063]: Odom: quality=74, std dev=0.045357m|0.045357rad, update time=0.024633s
[ INFO] [1503570157.475395618]: [Kinect2Bridge::main] depth processing: ~2.96237ms (~337.568Hz) publishing rate: ~29.9855Hz
[ INFO] [1503570157.475434096]: [Kinect2Bridge::main] color processing: ~8.38907ms (~119.203Hz) publishing rate: ~29.9855Hz
[ WARN] (2017-08-24 18:22:37.582) OdometryF2M.cpp:199::computeTransform() Failed to find a transformation with the provided guess (xyz=0.027227,0.016813,-0.001074 rpy=0.031567,-0.011728,0.003095), trying again without a guess.
[ WARN] (2017-08-24 18:22:37.588) OdometryF2M.cpp:207::computeTransform() Trial with no guess still fail.
 
King regards

[matlabbe]

Hi,

What is the corresponding TF you set between the IMU and the kinect? The part:

<!-- IMU frame: taken from the example, adjust to real position! -->
  <node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
      args="-0.032 0.0 0.032 0.0 0.0 0.0 /camera_rgb_frame /imu_link 100" />

In this example, the IMU was on top (3.2 cm) of the RGB camera of the kinect (little behind the RGB optical lens -3.2 cm), with IMU's z up, x forward, y left (no rotation required). Visualize the TF in RVIZ to make sure the IMU is in correct orientation according to camera.

cheers,
Mathieu