Using laser_scan_matcher with long range 2D scanner mounted not horizontally

edit

Hello, I would like to use laser scans data recorded from a 2D scanner into laser_scan_matcher package in order to get a pose2D or odometry (purely laser odometry). (Ros kinetic on Ubuntu 16.04) The scanner is mounted on the top of a vehicle at a particular orientation. We have a transformation matrix from the laser coordinate frame to the base frame (fixed to the vehicle) call it C_socs2base. I am using a launch file created from the demo launch file of the package.

Problem: I see that the base frame is not moving significantly and the trajectory is very short just centered around the origin. I am not getting any meaningful pose/trajectory.

Questions: Q1. Can laser_scan_matcher package work fine with a scanner sensor which is not mounted horizontally to the base coordinate? Note: The maximum range of the scanner is upto 200m

Q2. Do I need to initialize the first pose of the robot (base coordinate) in the world frame? If yes then how? I have done this step as discussed below: We have an initial pose of the base frame with respect to the world frame (x,y,z and roll,pitch, yaw at time 0 as t_0 ).

I edited the source code of the package in laser_scan_matcher.cpp where I replaced f2b_.setIdentity() and f2b_kf_.setIdentity() with the following codes:

tf::Quaternion q;

q.setOrigin(Vector3(x,y,z at t_0));

q.setRPY(roll, pitch, yaw at t_0);

f2b_.setRotation(q);

f2b_kf_ =  f2b_;enter code here`

Here is the launch file:

<launch>
  <arg name="IS_TWISTSTAMPED" default="true" />
  <arg name="use_rviz" default="true" />
  <arg name="publish_covariance" default="false"/>

  <!--set up data playback from bag /-->
  <param name="/use_sim_time" value="true"/>
  <param name="/stamped_vel" value="$(arg IS_TWISTSTAMPED)"/>

  <group if="$(arg use_rviz)">
    <node pkg="rviz" type="rviz" name="rviz"
          args="-d $(find laser_scan_matcher)/demo/demo.rviz"/>
  </group>


   <!-- Odometry from pose2D /-->
  <node pkg="pointcloud2_riegl" type="pose2odom" name="pose2odom" output="screen"/>

  <!--publish an example base_link -> laser transform /-->

  <node name = "SM_laser_bl" pkg="tf" type="static_transform_publisher" args="0.0772 -0.3409 -0.1009 0.4435054 0.7681739 -0.399886 -0.2308743 base_link laser 40"/>

  <!--start the laser scan_matcher -------------------------/>

  <group if="$(arg publish_covariance)">
    <param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
  </group>
  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
    name="laser_scan_matcher_node" output="screen">
    <remap from="scan" to="laser_scan" />
    <param name="max_iterations" value="10"/>
    <param name="publish_pose_stamped" value="true"/>

  </node>

</launch>

Thanks!