Are ObservationRangeBeacon measures used on localization?

[corot]

Hi all, I'm trying to incorporate mrpt_msgs/ObservationRangeBeacon measures in mrpt_localization. Filter clearly gets updated (I don't use laser input anymore and still particles dance) but seems to ignore the range beacon measures:

The white arrow represents the robot pose relative to the beacon. I move 20 cm every 5 seconds but nothing changes in the filter. Moreover, particles get more and more disperse with time:

Is there any problem with this feature? Or do I need to change something in the configuration? The mrpt.ini file seems fine to me:

[MetricMap]
# Creation of maps:
occupancyGrid_count=1
gasGrid_count=0
landmarksMap_count=0
pointsMap_count=0
beaconMap_count=0

# Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3)
likelihoodMapSelection=-1

# Enables (1) / Disables (0) insertion into specific maps:
enableInsertion_pointsMap=1
enableInsertion_landmarksMap=1
enableInsertion_gridMaps=1
enableInsertion_gasGridMaps=1
enableInsertion_beaconMap=1

My range beacon messages looks like this:

header: 
  seq: 23673
  stamp: 
    secs: 12081
    nsecs: 400000000
  frame_id: Seg_54W1_1
sensor_pose_on_robot: 
  position: 
    x: 0.0
    y: 0.0
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
min_sensor_distance: 0.2
max_sensor_distance: 1.0
sensor_std_range: 0.015
sensed_data: 
  - 
    range: 0.586409630196
    id: -1

where Seg_54W1_1 is the frame of the beacon.

Btw, if this works, I'll also implement a callback for ObservationRangeBearing. I suppose there's no reason to use ranges but not range + bearing, right?

[jlblancoc]

Hi! Thanks for the super detailed report!

I don't have much time for a long answer, but in short:

1) Take a look at this example config file: https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/pf-localization/ro-localization-demo.ini#L100

for range-only localization. Your ini file should be changed to include a "landmarks map" and the locations of each beacon.

2) The current implementation (I'm talking from what I remember, it was implemented ~10 years ago!) for SLAM includes data-association but pf-localization does not (!... yes, I know it's supposed to be an easier problem, it just wasn't implemented). If it's not a problem in your case, please, populate the "id" field of each sensed_data for all to work right.

3) Again, without looking at the code and talking from what I recall, I think ObservationRangeBearing's likelihood function wasn't implemented in any metric map so it couldn't be used to localization. Obviously, if it's not, the implementation should be easy so if you are interested I'll assist you in incorporating it to MRPT sources.

Cheers,

[corot]

Thanks a lot for the information. I tried the .ini file but then mrpt_localization crashes:

[/mrpt_localization_node  INFO 1478724202.789099383, 9.300000000]: InitializeFilter: 0.000m, 0.000m, 0.000rad 
terminate called after throwing an instance of 'std::logic_error'
  what():  

 =============== MRPT EXCEPTION =============
void mrpt::slam::CMonteCarloLocalization2D::resetUniformFreeSpace(mrpt::maps::COccupancyGridMap2D*, double, int, double, double, double, double, double, double), line 285:
Assert condition failed: nFreeCells
void mrpt::slam::CMonteCarloLocalization2D::resetUniformFreeSpace(mrpt::maps::COccupancyGridMap2D*, double, int, double, double, double, double, double, double), line 309:

It's strange to me that the map is of type landmarksMap, but then inserts beacons, no landmarks. Can be this an error? Because in fact there's a beaconMap too! I wonder if the format is not valid anymore, as the code evolves so fast.

Anyway, I'll debug tomorrow to get more details about the crash.

Thanks!

[jlblancoc]

Hi ,

That exception is due to having a gridmap without any free (unoccupied) space in it...

If you are only using a "landmarks map" and no gridmap, set the number of gridmaps in the ini file to zero.

Let us know if that solves the problem...

[corot]

Right! I see now that the exception has nothing to do with the landmarks map. But... this is quite bad! why should it be a problem to have a costmap without zero-cost cells? This must be a very recent change, in fact, that came maliciously to confuse itself with my experiments with beacons. I'll take a look and try to revert, unless there's a good reason for the change!

Coming back to your example, we plan to combine both laser localization with landmarks, so I suppose we need both maps.

In fact, what we want is to incorporate poses measured relative to some tfs, so we wouldn't need any landmark map at all. I think current implementation cannot fit this usecase, right? What do you think is the best way to accomplish this?

2016-11-15 11:44 GMT+01:00 Jose Luis Blanco-Claraco < notifications@github.com>:

Hi ,

That exception is due to having a gridmap without any free (unoccupied) space in it...

If you are only using a "landmarks map" and no gridmap, set the number of gridmaps in the ini file to zero.

Let us know if that solves the problem...

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/mrpt-ros-pkg/mrpt_navigation/issues/43#issuecomment-260608726, or mute the thread https://github.com/notifications/unsubscribe-auth/AATsMkSerXNzNUjnU8SyP5tSo-xd66MDks5q-Y0kgaJpZM4KsmP- .

[corot]

Hi, finally I have time to come to this issue! So... I can now add a single beacon and relocate nicely relative to it. The beacon is represented by the tf Seg_54V_0, and the particles spread at ~70cm from it, as expected:

As I explained you before, I'm not really using beacons but full poses calculated independently of mrpt, so by merging only distance I'm discarding very valuable information. Integrating ObservationRangeBearing sounds like the obvious next step... unless you think I should directly go for integrating full poses, I suppose using CObservation6DFeatures. That would be the most meaningful observation for my scenario: my sensors are able to localize in 6D external features. So what's your opinion? Could you give some hints on where and how I must modify the code?

Many thanks!

[jlblancoc]

Hi!

As discussed earlier, all that it takes to fuse information of new sensors into an MRPT's map, is to add a "switch()" case for that type of observation into the corresponding metric map's virtual method internal_computeObservationLikelihood() and internal_canComputeObservationLikelihood().

One example: https://github.com/MRPT/mrpt/blob/master/libs/maps/src/maps/CBeaconMap.cpp#L172

Hope this helps a bit...

[corot]

Sure! Yes, that's what I'm doing, but I added the likelihood function to a the CLandmarksMap, as is the one I use together with the OccupancyGridMap. Things seem to work with just pose and heading (yaw), but I'm having problems integrating the observation's covariance. Here is my attempt:

Eigen::VectorXd v_diff(6);
v_diff[0] = it->pose.x()     - robotPose3D.x();
v_diff[1] = it->pose.y()     - robotPose3D.y();
v_diff[2] = it->pose.z()     - robotPose3D.z();
v_diff[3] = 0;//it->pose.roll()  - robotPose3D.roll();
v_diff[4] = 0;//it->pose.pitch() - robotPose3D.pitch();
v_diff[5] = it->pose.yaw()   - robotPose3D.yaw();  // TODO normalize!!!

// Using this dist to estimate ret works!
double dist = sqrt( dij(0,0)*dij(0,0) + dij(0,1)*dij(0,1) + dij(0,2)*dij(0,2)
              + dij(0,3)*dij(0,3) + dij(0,4)*dij(0,4) + dij(0,5)*dij(0,5));

// But cannot integrate covariance properly...  this crash due to NaN values
Cij = CMatrixDouble(it->inf_matrix);
double distMahaFlik2 =  mahalanobisDistance(v_diff, Cij); //( dij * Cij_1 * (~dij) )(0,0);
// I also tried this:
double distMahaFlik2 =  dij.multiply_HCHt_scalar(Cij_1); //( dij * Cij_1 * (~dij) )(0,0);

ret = exp( -0.5 * distMahaFlik2 );

What do you think I should use? mahalanobisDistance? mahalanobisDistance2? In noticed they invert the covariance matrix... I think this can causing troubles, as my covariance matrices are not invertible by default (there are zeros in the diagonal for non estimated dimensions, z, roll and pitch)

That said, I noticed that none of the observation classes already available fits my needs: the information I'm integrating is a full robot pose provided by an external localization algorithm. I'm abusing now the CObservation6DFeatures class for doing that, but is something misleading, because we do this matching to external features outside MRPT (and also we cannot integrate orientation information by matching landmarks in a landmark map, as they only contain a 3D point). Instead, given you want to integrate my usecase in mrpt localization, I should add a new observation, something like CObservationRobotPose, and calculate the likelihood just comparing particles poses with this observed pose. The rational behind this usecase, is that till now we have been using robot_localization for fusing mrpt localization with other more local localization sources (using the sensors in the robot other than lasers), but this added a second filtering level (EKF) on top of the MRPT particle filters. We want instead use just one filter, the PF of mrpt_localizaiton, but still being able to integrate the other localization sources we already have. Do you think this is a valuable addition to MRPT? Or it's way too specific solution to fit in general-use libs as MRPT?

Sorry a lot if all this is confusing... I don't really understand MRPT internals. Maybe we can have a talk by skype or chat to clarify the blurry issues!

[corot]

OK, all working now. This is the likelihood calculation, much simpler than expected:

...
    } // end of likelihood of CObservationBeaconRanges
    else
    if ( CLASS_ID(CObservationRobotPose)==obs->GetRuntimeClass() )
    {
        /********************************************************************

                OBSERVATION TYPE: CObservationRobotPose

                Lik. between "this" and "robotPose";

        ********************************************************************/
        const CObservationRobotPose     *o = static_cast<const CObservationRobotPose*>( obs );

        // Compute the 3D position of the sensor:
        CPose3D sensorPose3D = robotPose3D + o->sensorPose;

        // Compute the likelihood according to mahalanobis distance:
        // Distance between poses:
        CMatrixD dij(1,6), Cij(6,6), Cij_1;
        dij(0,0) =          o->pose.x()     - sensorPose3D.x();
        dij(0,1) =          o->pose.y()     - sensorPose3D.y();
        dij(0,2) =          o->pose.z()     - sensorPose3D.z();
        dij(0,3) = wrapToPi(o->pose.roll()  - sensorPose3D.roll());
        dij(0,4) = wrapToPi(o->pose.pitch() - sensorPose3D.pitch());
        dij(0,5) = wrapToPi(o->pose.yaw()   - sensorPose3D.yaw());

        // Equivalent covariance from "i" to "j":
        Cij = CMatrixDouble(o->cov);
        Cij_1 = Cij.inv();

        double distMahaFlik2 =  dij.multiply_HCHt_scalar(Cij_1); //( dij * Cij_1 * (~dij) )(0,0);
        double ret = -0.5f * distMahaFlik2;

        MRPT_CHECK_NORMAL_NUMBER(ret);
        return ret;

    } // end of likelihood of CObservation
    else
    if ( CLASS_ID(CObservationGPS)==obs->GetRuntimeClass() )
    {
...

As you can see, the new CObservationRobotPose class only contains the pose, its covariance and the sensor pose. As I said before, I added to CLandmarksMap, what maybe it's not the right place... I need guidance here. I can do tentative PRs to mrpt and mrpt_navigation so someone can give it a try and evaluate if it's a worthy addition to MRPT.

[corot]

Thanks! I think we can close this