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

remove all using namespace

Browse Source
This commit is contained in:
Jeremie Deray
2017-05-16 12:13:19 +02:00
committed by Jeremie Deray
parent c3d22b8780
commit dec57a6461
3 changed files with 31 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -32,7 +32,7 @@
#else #else
# include <mrpt/slam/CObservation2DRangeScan.h> # include <mrpt/slam/CObservation2DRangeScan.h>
# include <mrpt/slam/CObservationOdometry.h> # include <mrpt/slam/CObservationOdometry.h>
using namespace mrpt::slam;
#endif #endif
#if MRPT_VERSION<0x150 #if MRPT_VERSION<0x150
@@ -169,7 +169,7 @@ protected:
void solveSystemNonLinear(); void solveSystemNonLinear();
void filterLevelSolution(); void filterLevelSolution();
void PoseUpdate(); void PoseUpdate();
void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan); void Reset(mrpt::poses::CPose3D ini_pose, mrpt::poses::CObservation2DRangeScan scan);
}; };
#endif #endif

62
src/CLaserOdometry2D.cpp
View File

@@ -14,13 +14,6 @@
******************************************************************************************** */ ******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h" #include "rf2o_laser_odometry/CLaserOdometry2D.h"
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;
using namespace Eigen;
// -------------------------------------------- // --------------------------------------------
// CLaserOdometry2D // CLaserOdometry2D
@@ -468,8 +461,8 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface()
for (unsigned int u = 0; u < cols_i-1; u++) for (unsigned int u = 0; u < cols_i-1; u++)
{ {
const float dist = square(xx_inter[image_level](u+1) - xx_inter[image_level](u)) const float dist = mrpt::math::square(xx_inter[image_level](u+1) - xx_inter[image_level](u))
+ square(yy_inter[image_level](u+1) - yy_inter[image_level](u)); + mrpt::math::square(yy_inter[image_level](u+1) - yy_inter[image_level](u));
if (dist > 0.f) if (dist > 0.f)
rtita(u) = sqrt(dist); rtita(u) = sqrt(dist);
} }
@@ -545,7 +538,7 @@ void CLaserOdometry2D::computeWeights()
//Parameters for error_linearization //Parameters for error_linearization
const float kdtita = 1.f; const float kdtita = 1.f;
const float kdt = kdtita/square(fps); const float kdt = kdtita/mrpt::math::square(fps);
const float k2d = 0.2f; const float k2d = 0.2f;
for (unsigned int u = 1; u < cols_i-1; u++) for (unsigned int u = 1; u < cols_i-1; u++)
@@ -559,7 +552,7 @@ void CLaserOdometry2D::computeWeights()
const float dtitat = ini_dtita - final_dtita; const float dtitat = ini_dtita - final_dtita;
const float dtita2 = dtita(u+1) - dtita(u-1); const float dtita2 = dtita(u+1) - dtita(u-1);
const float w_der = kdt*square(dt(u)) + kdtita*square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2)); const float w_der = kdt*mrpt::math::square(dt(u)) + kdtita*mrpt::math::square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2));
weights(u) = sqrt(1.f/w_der); weights(u) = sqrt(1.f/w_der);
} }
@@ -615,13 +608,13 @@ void CLaserOdometry2D::solveSystemOneLevel()
} }
//Solve the linear system of equations using a minimum least squares method //Solve the linear system of equations using a minimum least squares method
MatrixXf AtA, AtB; Eigen::MatrixXf AtA, AtB;
AtA.multiply_AtA(A); AtA.multiply_AtA(A);
AtB.multiply_AtB(A,B); AtB.multiply_AtB(A,B);
Var = AtA.ldlt().solve(AtB); Var = AtA.ldlt().solve(AtB);
//Covariance matrix calculation Cov Order -> vx,vy,wz //Covariance matrix calculation Cov Order -> vx,vy,wz
MatrixXf res(num_valid_range,1); Eigen::MatrixXf res(num_valid_range,1);
res = A*Var - B; res = A*Var - B;
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
@@ -657,13 +650,13 @@ void CLaserOdometry2D::solveSystemNonLinear()
} }
//Solve the linear system of equations using a minimum least squares method //Solve the linear system of equations using a minimum least squares method
MatrixXf AtA, AtB; Eigen::MatrixXf AtA, AtB;
AtA.multiply_AtA(A); AtA.multiply_AtA(A);
AtB.multiply_AtB(A,B); AtB.multiply_AtB(A,B);
Var = AtA.ldlt().solve(AtB); Var = AtA.ldlt().solve(AtB);
//Covariance matrix calculation Cov Order -> vx,vy,wz //Covariance matrix calculation Cov Order -> vx,vy,wz
MatrixXf res(num_valid_range,1); Eigen::MatrixXf res(num_valid_range,1);
res = A*Var - B; res = A*Var - B;
//cout << endl << "max res: " << res.maxCoeff(); //cout << endl << "max res: " << res.maxCoeff();
//cout << endl << "min res: " << res.minCoeff(); //cout << endl << "min res: " << res.minCoeff();
@@ -684,7 +677,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
const float k = 10.f/aver_dt; //200 const float k = 10.f/aver_dt; //200
//float energy = 0.f; //float energy = 0.f;
//for (unsigned int i=0; i<res.rows(); i++) //for (unsigned int i=0; i<res.rows(); i++)
// energy += log(1.f + square(k*res(i))); // energy += log(1.f + mrpt::math::square(k*res(i)));
//printf("\n\nEnergy(0) = %f", energy); //printf("\n\nEnergy(0) = %f", energy);
//Solve iterative reweighted least squares //Solve iterative reweighted least squares
@@ -696,7 +689,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
for (unsigned int u = 1; u < cols_i-1; u++) for (unsigned int u = 1; u < cols_i-1; u++)
if (null(u) == 0) if (null(u) == 0)
{ {
const float res_weight = sqrt(1.f/(1.f + square(k*res(cont)))); const float res_weight = sqrt(1.f/(1.f + mrpt::math::square(k*res(cont))));
//Fill the matrix Aw //Fill the matrix Aw
Aw(cont,0) = res_weight*A(cont,0); Aw(cont,0) = res_weight*A(cont,0);
@@ -715,17 +708,18 @@ void CLaserOdometry2D::solveSystemNonLinear()
////Compute the energy ////Compute the energy
//energy = 0.f; //energy = 0.f;
//for (unsigned int j=0; j<res.rows(); j++) //for (unsigned int j=0; j<res.rows(); j++)
// energy += log(1.f + square(k*res(j))); // energy += log(1.f + mrpt::math::square(k*res(j)));
//printf("\nEnergy(%d) = %f", i, energy); //printf("\nEnergy(%d) = %f", i, energy);
} }
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
kai_loc_level = Var; kai_loc_level = Var;
if (verbose) if (verbose)
std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl; std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl;
} }
void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan) void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, mrpt::poses::CObservation2DRangeScan scan)
{ {
//Set the initial pose //Set the initial pose
laser_pose = ini_pose; laser_pose = ini_pose;
@@ -741,11 +735,12 @@ void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
void CLaserOdometry2D::performWarping() void CLaserOdometry2D::performWarping()
{ {
Matrix3f acu_trans; Matrix3f acu_trans;
acu_trans.setIdentity(); acu_trans.setIdentity();
for (unsigned int i=1; i<=level; i++) for (unsigned int i=1; i<=level; i++)
acu_trans = transformations[i-1]*acu_trans; acu_trans = transformations[i-1]*acu_trans;
MatrixXf wacu(1,cols_i); Eigen::MatrixXf wacu(1,cols_i);
wacu.assign(0.f); wacu.assign(0.f);
range_warped[image_level].assign(0.f); range_warped[image_level].assign(0.f);
@@ -781,11 +776,11 @@ void CLaserOdometry2D::performWarping()
} }
else else
{ {
const float w_r = square(delta_l); const float w_r = mrpt::math::square(delta_l);
range_warped[image_level](uwarp_r) += w_r*range_w; range_warped[image_level](uwarp_r) += w_r*range_w;
wacu(uwarp_r) += w_r; wacu(uwarp_r) += w_r;
const float w_l = square(delta_r); const float w_l = mrpt::math::square(delta_r);
range_warped[image_level](uwarp_l) += w_l*range_w; range_warped[image_level](uwarp_l) += w_l*range_w;
wacu(uwarp_l) += w_l; wacu(uwarp_l) += w_l;
} }
@@ -830,18 +825,18 @@ void CLaserOdometry2D::filterLevelSolution()
//First, we have to describe both the new linear and angular speeds in the "eigenvector" basis //First, we have to describe both the new linear and angular speeds in the "eigenvector" basis
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
Matrix<float,3,3> Bii; Eigen::Matrix<float,3,3> Bii;
Matrix<float,3,1> kai_b; Eigen::Matrix<float,3,1> kai_b;
Bii = eigensolver.eigenvectors(); Bii = eigensolver.eigenvectors();
kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level); kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level);
//Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
CMatrixFloat31 kai_loc_sub; mrpt::math::CMatrixFloat31 kai_loc_sub;
//Important: we have to substract the solutions from previous levels //Important: we have to substract the solutions from previous levels
Matrix3f acu_trans; Eigen::Matrix3f acu_trans;
acu_trans.setIdentity(); acu_trans.setIdentity();
for (unsigned int i=0; i<level; i++) for (unsigned int i=0; i<level; i++)
acu_trans = transformations[i]*acu_trans; acu_trans = transformations[i]*acu_trans;
@@ -851,16 +846,16 @@ void CLaserOdometry2D::filterLevelSolution()
if (acu_trans(0,0) > 1.f) if (acu_trans(0,0) > 1.f)
kai_loc_sub(2) = 0.f; kai_loc_sub(2) = 0.f;
else else
kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*sign(acu_trans(1,0)); kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
kai_loc_sub += kai_loc_old; kai_loc_sub += kai_loc_old;
Matrix<float,3,1> kai_b_old; Eigen::Matrix<float,3,1> kai_b_old;
kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub); kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);
//Filter speed //Filter speed
const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level)); const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level));
Matrix<float,3,1> kai_b_fil; Eigen::Matrix<float,3,1> kai_b_fil;
for (unsigned int i=0; i<3; i++) for (unsigned int i=0; i<3; i++)
{ {
kai_b_fil(i,0) = (kai_b(i,0) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i,0))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df); kai_b_fil(i,0) = (kai_b(i,0) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i,0))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df);
@@ -868,7 +863,7 @@ void CLaserOdometry2D::filterLevelSolution()
} }
//Transform filtered speed to local reference frame and compute transformation //Transform filtered speed to local reference frame and compute transformation
Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); Eigen::Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);
//transformation //transformation
const float incrx = kai_loc_fil(0)/fps; const float incrx = kai_loc_fil(0)/fps;
@@ -887,7 +882,7 @@ void CLaserOdometry2D::PoseUpdate()
{ {
//First, compute the overall transformation //First, compute the overall transformation
//--------------------------------------------------- //---------------------------------------------------
Matrix3f acu_trans; Eigen::Matrix3f acu_trans;
acu_trans.setIdentity(); acu_trans.setIdentity();
for (unsigned int i=1; i<=ctf_levels; i++) for (unsigned int i=1; i<=ctf_levels; i++)
acu_trans = transformations[i-1]*acu_trans; acu_trans = transformations[i-1]*acu_trans;
@@ -900,7 +895,7 @@ void CLaserOdometry2D::PoseUpdate()
if (acu_trans(0,0) > 1.f) if (acu_trans(0,0) > 1.f)
kai_loc(2) = 0.f; kai_loc(2) = 0.f;
else else
kai_loc(2) = fps*acos(acu_trans(0,0))*sign(acu_trans(1,0)); kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
//cout << endl << "Arc cos (incr tita): " << kai_loc(2); //cout << endl << "Arc cos (incr tita): " << kai_loc(2);
@@ -914,6 +909,7 @@ void CLaserOdometry2D::PoseUpdate()
// Update poses // Update poses
//------------------------------------------------------- //-------------------------------------------------------
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 + CPose3D(pose_aux_2D); laser_pose = laser_pose + CPose3D(pose_aux_2D);
@@ -945,7 +941,7 @@ void CLaserOdometry2D::PoseUpdate()
double time_inc_sec = (current_scan_time - last_odom_time).toSec(); double time_inc_sec = (current_scan_time - last_odom_time).toSec();
last_odom_time = current_scan_time; last_odom_time = current_scan_time;
lin_speed = acu_trans(0,2) / time_inc_sec; lin_speed = acu_trans(0,2) / time_inc_sec;
//double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec; //double lin_speed = sqrt( mrpt::math::square(robot_oldpose.x()-robot_pose.x()) + mrpt::math::square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;
double ang_inc = robot_pose.yaw() - robot_oldpose.yaw(); double ang_inc = robot_pose.yaw() - robot_oldpose.yaw();
if (ang_inc > 3.14159) if (ang_inc > 3.14159)
ang_inc -= 2*3.14159; ang_inc -= 2*3.14159;

6
src/CLaserOdometry2DNode.cpp
View File

@@ -14,12 +14,6 @@
******************************************************************************************** */ ******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h" #include "rf2o_laser_odometry/CLaserOdometry2D.h"
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;
using namespace Eigen;
class CLaserOdometry2DNode : CLaserOdometry2D class CLaserOdometry2DNode : CLaserOdometry2D
{ {