Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

When I placed the mid360 upside down, the localization of Fast LIO drifted away. #284

Closed
66Lau opened this issue Nov 16, 2023 · 4 comments
Closed

Comments

@66Lau
Copy link

66Lau commented Nov 16, 2023

Hi!
When I placed the mid360 upside down and modified the roll axis to 180 degrees in the Livox config file, the localization of Fast LIO drifted away. Did I miss some steps when I trying to place mid360 up side down?

@Ecstasy-EC
Copy link
Member

What do you mean by "modified the roll axis to 180 degrees" in the config file?

@66Lau
Copy link
Author

66Lau commented Nov 19, 2023

Sorry, for ambiguity, I mean I changed the extrinsic_parameter in MID360_config.json like the code below.

"lidar_configs" : [
    {
      "ip" : "192.168.1.12",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
        "roll": 180,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]

However, I found that this extrinsic_parameter can only change the pose of the pointclouds, but not IMU. As a result, the localization drifted away. When I try to use mid360 build the map in the tilted installation or inverted installation, the PCD looks very strange because it takes camera_init as the base coordinates.
slope_down_37_degree

So I want to modify the code to map the imu in the inclined state to the horizontal state

I may have found a way to achieve this, just use static_transform_publisher to publish the transfrom from camera_init to foot_init if the installation of mid360 is not vertical and take the foot_init as the base coordinates
37_degree_adjust
tftree
But is not the routine method mentioned in navigationTutorialsRobotSetupTF that "we need a way of transforming the laser scan we've received from the "base_laser" frame to the "base_link" frame."

Is there any better way?

@Ecstasy-EC
Copy link
Member

As we build the map in the IMU frame, I would suggest conducting a gravity alignment during the initialization step. Read the IMU to identify the gravity direction, and record a transformation matrix to translate your IMU to the gravity frame. After that, you only need to transform the point cloud via that matrix when publish the point clouds. You'll obtain the point cloud in the gravity frame by doing so.

@66Lau
Copy link
Author

66Lau commented Nov 19, 2023

Make sense, appreciate!!

@66Lau 66Lau closed this as completed Nov 19, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants