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

fix indentation + credits

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-16 10:03:13 +02:00
parent f159d3eb2e
commit 77fb8e3a86
3 changed files with 779 additions and 789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -11,6 +11,8 @@
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */
#ifndef CLaserOdometry2D_H
@@ -158,7 +160,6 @@ protected:
std::vector<double> last_m_lin_speeds;
std::vector<double> last_m_ang_speeds;
// Methods
void createImagePyramid();
void calculateCoord();

21
src/CLaserOdometry2D.cpp
View File

@@ -11,6 +11,8 @@
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
@@ -234,7 +236,6 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
return true;
}
void CLaserOdometry2D::createImagePyramid()
{
const float max_range_dif = 0.3f;
@@ -405,8 +406,6 @@ void CLaserOdometry2D::createImagePyramid()
}
}
void CLaserOdometry2D::calculateCoord()
{
for (unsigned int u = 0; u < cols_i; u++)
@@ -426,7 +425,6 @@ void CLaserOdometry2D::calculateCoord()
}
}
void CLaserOdometry2D::calculaterangeDerivativesSurface()
{
//The gradient size ir reserved at the maximum size (at the constructor)
@@ -485,7 +483,6 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface()
//dtmed.swap(dt);
}
void CLaserOdometry2D::computeNormals()
{
normx.setConstant(1, cols, 0.f);
@@ -513,7 +510,6 @@ void CLaserOdometry2D::computeNormals()
}
}
void CLaserOdometry2D::computeWeights()
{
//The maximum weight size is reserved at the constructor
@@ -546,7 +542,6 @@ void CLaserOdometry2D::computeWeights()
weights = inv_max*weights;
}
void CLaserOdometry2D::findNullPoints()
{
//Size of null matrix is set to its maximum size (constructor)
@@ -564,7 +559,6 @@ void CLaserOdometry2D::findNullPoints()
}
}
// Solves the system without considering any robust-function
void CLaserOdometry2D::solveSystemOneLevel()
{
@@ -607,7 +601,6 @@ void CLaserOdometry2D::solveSystemOneLevel()
kai_loc_level_ = Var;
}
// Solves the system by considering the Cauchy M-estimator robust-function
void CLaserOdometry2D::solveSystemNonLinear()
{
@@ -657,7 +650,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
aver_res += std::abs(res(ind++));
}
aver_dt /= cont; aver_res /= cont;
// printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res);
// printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res);
const float k = 10.f/aver_dt; //200
@@ -716,7 +709,6 @@ void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan s
createImagePyramid();
}
void CLaserOdometry2D::performWarping()
{
Eigen::Matrix3f acu_trans;
@@ -792,10 +784,6 @@ void CLaserOdometry2D::performWarping()
}
}
void CLaserOdometry2D::filterLevelSolution()
{
// Calculate Eigenvalues and Eigenvectors
@@ -864,7 +852,6 @@ void CLaserOdometry2D::filterLevelSolution()
transformations[level](1,2) = incry;
}
void CLaserOdometry2D::PoseUpdate()
{
//First, compute the overall transformation
@@ -898,7 +885,7 @@ void CLaserOdometry2D::PoseUpdate()
//-------------------------------------------------------
laser_oldpose_ = laser_pose_;
// Eigen::Matrix3f aux_acu = acu_trans;
// Eigen::Matrix3f aux_acu = acu_trans;
Pose3d pose_aux_2D = Pose3d::Identity();
pose_aux_2D = rf2o::matrixYaw(double(kai_loc_(2)/fps));

2
src/CLaserOdometry2DNode.cpp
View File

@@ -11,6 +11,8 @@
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h"