Strange laser_scan_matcher performance [closed]
I've been trying to use laser_scan_matcher(lsm) package to be used for better estimating height of a quadcopter. A laser scanner is mounted as shown in the sketch, i.e, the mirror is pointing to the earth. +x is of the laser frame axis and -z is of the quadcopter frame axis.
I wonder why the output of the lsm is very small as it's not supposed to be(Please refer to the provided rxgraph).
The way I implement the height estimation is first I initialize the height value by using barometer reading(only once using barometer reading). Then, over time I add the initialized barometer reading with the difference between the current and previous lsm of x-component respectively. But, unfortunately I got a very small of x-value over time and that shows in the graph, the value of height is just constant(of red line). By the way, the blue one is solely from barometer reading. I don't think lsm performs this bad..
What could be wrong in this regard? Is there such a restriction in the lsm that won't work with this kind of hardware configuration. What I mean, it could be the case lsm algorithm is expecting the laser scanner has something to be hit sideways.
Thanks in advance.
EDIT:
At C++ level, I integrate them as follows:
pose3D_combined_.pose.position.z -= (current_pose2D_.x - x_pose2D_old_)*cos(pcl::deg2rad(current_angle_altitude_.angle_pitch));
where,
pose3D_combined_.pose.position.z is the height(Initialized with barometer reading once)
current_pose2D_.x is the current x-value of lsm
x_pose2D_old_ is is the previous x-value of lsm
current_angle_altitude_.angle_pitch is the current pitch angle
The output of rostopic echo /pose2D:
x: -0.000789300255834
y: -5.79076165633
theta: -1.8436533323e-05
---
x: -0.000796823306055
y: -5.89912052651
theta: -1.74014890123e-05
---
x: -0.000814190304881
y: -5.96790343469
theta: -2.16632225298e-05
---
x: -0.00081592369
y: -6.04254708239
theta: -2.28163634433e-05
---
x: -0.000819459389021
y: -6.08864746421
theta: -2.49307842707e-05
---
x: -0.000826405097133
y: -6.12202770971
theta: -2.50607983819e-05
---
x: -0.000834465217375
y: -6.15494752541
theta: -2.49607931812e-05
---
I'm not sure what the issue is. Can you post a link to a higher-resolution picture so I can see the units? Also, have you looked at laser_height_estimation from mav tools? It might be better suited for your needs.
The issue is the output from lsm if far smaller than expected(see the red line), because it should at least follow the pattern in blue(from barometer). Do you think lsm won't be working for my system, because with this, the laser scanner is moving in z-axis as opposed to in a ground robot case?
it should work regardless of the axis. what are the units of the output of the barometer?
The barometer output is in meter as well, the same as the lsm output.
@Ivan Dryanovski: I've updated my code snippet that implements the integration of the values. Does that make sense, you think?
Did you supply a tf for the laser->base_link frame that correctly shows the fact that the laser is pointed at the ground (eg 90deg pitch)? You may want to check the x and y coordinates of the output pose to make sure that the scan matcher isn't adjusting say, the x value of the pose instead of z.
@Eric Perko: Yes, I supply the TF for laser->base_link, and in my case, I use static_transform_publisher to realize that. For your 2nd point, I didn't get it, sorry.
@alfa_80: Now that you've updated your answer with the code, I see the second part of my comment isn't relevant, since you are already treating the x-coordinate from the scan matcher as the z-coord in your pose estimate (but that may or may not be incorrect if you included the pitch in your TF).