mirror of
https://github.com/MAPIRlab/rf2o_laser_odometry.git
synced 2023-04-06 19:28:55 +08:00
Changes to make it work with MRPT 1.9
- Added C++14 flag to CMakeLists.txt - Fixed conversion of CPoses
This commit is contained in:
@@ -1,5 +1,6 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
project(rf2o_laser_odometry)
|
project(rf2o_laser_odometry)
|
||||||
|
set (CMAKE_CXX_STANDARD 14) # Require C++14
|
||||||
|
|
||||||
## Find catkin macros and libraries
|
## Find catkin macros and libraries
|
||||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||||
|
|||||||
@@ -34,7 +34,11 @@
|
|||||||
# include <mrpt/slam/CObservationOdometry.h>
|
# include <mrpt/slam/CObservationOdometry.h>
|
||||||
using namespace mrpt::slam;
|
using namespace mrpt::slam;
|
||||||
#endif
|
#endif
|
||||||
#include <mrpt/system/threads.h>
|
|
||||||
|
#if MRPT_VERSION<0x150
|
||||||
|
# include <mrpt/system/threads.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
#include <mrpt/system/os.h>
|
#include <mrpt/system/os.h>
|
||||||
#include <mrpt/poses/CPose3D.h>
|
#include <mrpt/poses/CPose3D.h>
|
||||||
#include <mrpt/utils.h>
|
#include <mrpt/utils.h>
|
||||||
|
|||||||
@@ -144,8 +144,8 @@ void CLaserOdometry2D::Init()
|
|||||||
|
|
||||||
|
|
||||||
//Set the initial pose
|
//Set the initial pose
|
||||||
laser_pose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
|
laser_pose = robotInitialPose + LaserPoseOnTheRobot;
|
||||||
laser_oldpose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
|
laser_oldpose = robotInitialPose + LaserPoseOnTheRobot;
|
||||||
|
|
||||||
|
|
||||||
// Init module (internal)
|
// Init module (internal)
|
||||||
@@ -943,7 +943,7 @@ void CLaserOdometry2D::PoseUpdate()
|
|||||||
laser_oldpose = laser_pose;
|
laser_oldpose = laser_pose;
|
||||||
math::CMatrixDouble33 aux_acu = acu_trans;
|
math::CMatrixDouble33 aux_acu = acu_trans;
|
||||||
poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
|
poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
|
||||||
laser_pose = laser_pose + pose_aux_2D;
|
laser_pose = laser_pose + CPose3D(pose_aux_2D);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user