mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

moving headers & setLaserPose with CPose3D arg

Browse Source
This commit is contained in:
Jeremie Deray
2017-05-16 12:56:24 +02:00
committed by Jeremie Deray
parent dec57a6461
commit 5893d1a3e9
3 changed files with 47 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/CLaserOdometry2D.cpp
View File

@@ -32,22 +32,11 @@ CLaserOdometry2D::~CLaserOdometry2D()
//
}
void CLaserOdometry2D::setLaserPose(const tf::Transform& laser_pose)
void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& laser_pose)
{
//Set laser pose on the robot
//TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
const tf::Vector3 &t = laser_pose.getOrigin();
laser_pose_on_robot.x() = t[0];
laser_pose_on_robot.y() = t[1];
laser_pose_on_robot.z() = t[2];
const tf::Matrix3x3 &basis = laser_pose.getBasis();
mrpt::math::CMatrixDouble33 R;
for(int r = 0; r < 3; r++)
for(int c = 0; c < 3; c++)
R(r,c) = basis[r][c];
laser_pose_on_robot.setRotationMatrix(R);
laser_pose_on_robot = laser_pose;
laser_pose_on_robot_inv = laser_pose_on_robot;
laser_pose_on_robot_inv.inverse();
}