Using multiple (2) pose sources at once about ethzasl_msf HOT 15 OPEN

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_2_handler_);

• 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

from ethzasl_msf.

@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.

from ethzasl_msf.

@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).

from ethzasl_msf.

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?

from ethzasl_msf.

@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.)

from ethzasl_msf.

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.

from ethzasl_msf.

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

from ethzasl_msf.

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

from ethzasl_msf.

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" [email protected] 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
#80 (comment).

from ethzasl_msf.

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

Thanks!

from ethzasl_msf.

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

from ethzasl_msf.

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

from ethzasl_msf.

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?

from ethzasl_msf.

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.

from ethzasl_msf.

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

from ethzasl_msf.