Using multiple (2) pose sources at once

[mhkabir]

What would be the procedure if we were to use two pose sources? I have 2 visual SLAM sources onboard (forward looking and downward looking for mapping and odometry). I want to use the two sources simultaneously for robustness and redundancy.

Thanks, Kabir

[simonlynen]

@mhkabir The pose measurement is a template for the states it acts on. https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h#L45

So you can add two types of measurements which are originating from two different cameras.

[simonlynen]

@mhkabir You add the same states twice to your state definition which you need for a single pose sensor. The measurements are then instantiated according to the states they work on (calibration, scale etc).

[mhkabir]

Thank you.

Can you briefly tell me the code changes necessary for this? I am guessing they will be quite small. No changes to pose_sensor_handler will be required I suppose?

[mhkabir]

@simonlynen It'd be great if one of you guys could do a small wiki page or something outlining how to do this with the current system. I am willing to update the documentation for the other parts of the system if I can ge ssomething working here (and understand myself.)

[fdevillalobos]

I'm also trying to do something similar but stuck here. Willing to help if I can figure it out. I have a range measuring the distance to an object, and in a perpendicular plane I have a camera seeing the same object. They are both doing an estimate of the depth the object is at from the start plane (Where the range sensor is). Where do I have to feed both of these readings to get an accurate estimate? And where do I get that estimate published??

Thanks a lot! Willing to contribute to the project. Francisco.

[m42neb]

Hi everyone, I am a post-doc from Sapienza, University of Rome, and I am starting to use this framework to set up state estimation with our UAV. Let me first thank the developers for providing such a powerful, useful implementation.

Coming to the current topic, I am trying to implement a sensor manager to manage two different pose sensor (e.g. AR-marker + VSLAM). To do so I have modified the poseSensorManager to create a doublePoseSensorManager, in particular I did the following:

• changes in the msf_statedef.hpp to include states for the second pose sensor

• creation of a second sensor_handler in my doublePoseSensorManager, by passing the new states for the template, i.e. I declared a new typedef for the new pose_sensor with states corresponding to the new states (marked by a leading '_2')

  typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement<
    msf_updates::EKFState::StateDefinition_T::L_2,
    msf_updates::EKFState::StateDefinition_T::q_ic_2,
    msf_updates::EKFState::StateDefinition_T::p_ic_2,
    msf_updates::EKFState::StateDefinition_T::q_wv_2,
    msf_updates::EKFState::StateDefinition_T::p_wv_2
  >,DoublePoseSensorManager> PoseSensor_2_Handler_T;

  I created and added the new pose sensor with its handler: AddHandler(pose_2handler);

• I consequently adapted the 'Init' and the 'CalculateQAuxiliaryStates' functions to initialize and manage the new states (if I properly get, your framework should correctly manage the states from then on)
• I duplicated the parameters for the two sensors in the _fix.yaml file, which are just the same for the moment, since I would like to check the implementation before using it.

I think my implementation should be correct, since I see a correct initialization of the filter, I can actually modify the parameters for the two sensors separately, the number of states is correct, etc. I will be grateful if anyone can confirm the correctness of my steps.

To test my code I run the viconpos_sensor example generating a logfile with 'rostopic echo' with the whole state. Then I run my code, by remapping the vicon topic separately on each sensor, i.e. using only the first or the second pose sensor. In this way I should see the same output, since I have two equal sensors mapping the same measurements in the same way. The I compared the result (UAV position) of the example with the two results of my code. I get exactly the same trajectory of the example when running with the first pose sensor, while I get a slightly (but significantly) different trajectory when using the second pose sensor. Here come my questions:

• do I have a conceptual flaw? I think I should get the same results from two equal sensors, no matter where they are placed in the EKF state. If not, why?
• is there any fixed parameter I did not find out, which must be modified to include a second pose sensor?
• is there any further modification I am missing?

Sorry for the long post, but I think is better to include as much information as possible.

Thanks, have a nice day. Lorenzo

[m42neb]

Hi everyone, I solved the issue, which was due to a bad (indeed missing) initialization of the second sensor measurement noise (which is done in the dynamic reconfigure callback). Now I have the two sensors giving exactly the same results.

Lorenzo

[simonlynen]

@m42neb sorry for the delay and thanks for posting your solution here.

[mhkabir]

Hi, really like your results :)

Do you have it somewhere on a branch? It'd be very welcome :) On 30 Apr 2015 19:56, "Simon Lynen" notifications@github.com wrote:

@m42neb https://github.com/m42neb sorry for the delay and thanks for posting your solution here.

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/ethzasl_msf/issues/80#issuecomment-97817477.

[MarcGyongyosi]

never mind my questions - I was able to figure it out.

Thanks!

[snakehaihai]

Hi everyone, I solved the issue, which was due to a bad (indeed missing) initialization of the second sensor measurement noise (which is done in the dynamic reconfigure callback). Now I have the two sensors giving exactly the same results.

Lorenzo

Hi Lorenzo I`m fusing GPS vicon and UWB. all of them are 6DOF pose. Can I ask u how did u manage to do it without subscribing to the 2nd sensor in pose_sensorhandler.hpp?

Exactly how many file needs to be changeds? So far i added the new states for 2nd sensor in /home/snake/catkin_ws/src/ethzasl_msf/msf_updates/src/pose_msf/msf_statedef.hpp and added handler in /home/snake/catkin_ws/src/ethzasl_msf/msf_updates/src/pose_msf/pose_sensormanager.h

however, I really have no idea how u put the secondary sensor in. Can you help with me?

Regards Shenghai Yuan

[tiralonghipol]

Hi, what is the current status of the two poses handling? thanks

[Cristian-wp]

Hi, I am trying to perform the fusion between two pose sources: one from lidar slam and one from visual inertial slam. I have read the previous comment but I can not manage to make my setup work. There is a tutorial that explain the right method to perform the fusion? A dedicated branch or something like that? @simonlynen can you please explane the steps? @mhkabir have you find a working solution?

[snakehaihai]

you need to change the source code to do that. one easier option is to use robot_localization node that only need to specify in the config. that's a lot more easier.

[Cristian-wp]

Hi @snakehaihai ! Thanks a lot for your kind reply. I have already use robot_localization for my setup with really good results, but I wish to do the same with this package. Can you please give me some help? There is a ready implementation of the needed code changes? At the moment I am trying to follow @m42neb instruction (with the hope to better understand the code) but I am stuck