Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add std::cout << std::flush; fix bug in synchronizeLaserOdom(). #12

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add std::cout << std::flush; fix bug in synchronizeLaserOdom(). #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 2.8.3)
project(laser-odom_calibration)

# Detection of CSM library using CMake routines
Expand Down
8 changes: 4 additions & 4 deletions launch/example.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
<launch>

<node pkg="laser-odom_calibration" name="calib" type="io_test" output="screen">
<param name="file_path" type="string" value="/home/heyijia/ros_ws/OLCalibra_ws/" />
<param name="bagname" type="string" value="2019-02-26-12-01-45" />
<param name="laser_topic" type="string" value="/scan" />
<param name="odom_topic" type="string" value="/calibr_data" />
<param name="file_path" type="string" value="/home/txcom-ubuntu64/" />
<param name="bagname" type="string" value="2021-05-17-15-43-37" />
<param name="laser_topic" type="string" value="scan" />
<param name="odom_topic" type="string" value="odom" />
</node>

</launch>
43 changes: 22 additions & 21 deletions src/io.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,19 +46,20 @@ void messageIO::readDataFromBag(const std::string &bag_name, const std::string &
laser_data.push_back(tmp);
}

// nav_msgs::OdometryConstPtr odom = m.instantiate<nav_msgs::Odometry>();
// if (odom != NULL) {
// odometerData tmp;
// tmp.timestamp = odom->header.stamp;
// double r_l = 0.4;
// double r_r = 0.4;
// double b = 0.7;
// double v = odom->twist.twist.linear.x;
// double omega = odom->twist.twist.angular.z;
// tmp.v_l = (v / r_l) - ((omega * b) / (2 * r_l));
// tmp.v_r = (v / r_r) + ((omega * b) / (2 * r_r));
// odom_data.push_back(tmp);
// }
nav_msgs::OdometryConstPtr odom = m.instantiate<nav_msgs::Odometry>();
if (odom != NULL) {
odometerData tmp;
tmp.timestamp = odom->header.stamp;
double r_l = 0.2/2.0; //0.141/2.0;
double r_r = 0.2/2.0; //0.141/2.0;
double b = 0.48342; //0.42; //0.3819;
double v = odom->twist.twist.linear.x;
double omega = odom->twist.twist.angular.z;
//tmp.v_l tmp.v_r rad/s
tmp.v_l = (v / r_l) - ((omega * b) / (2 * r_l));
tmp.v_r = (v / r_r) + ((omega * b) / (2 * r_r));
odom_data.push_back(tmp);
}

// sensor_msgs::JointStateConstPtr odom = m.instantiate<sensor_msgs::JointState>();
// if (odom != NULL) {
Expand All @@ -69,14 +70,14 @@ void messageIO::readDataFromBag(const std::string &bag_name, const std::string &
// odom_data.push_back(tmp);
// }
//
geometry_msgs::Vector3StampedConstPtr odom = m.instantiate<geometry_msgs::Vector3Stamped>();
if (odom != NULL) {
odometerData tmp;
tmp.timestamp = odom->header.stamp;
tmp.v_l = odom->vector.x;
tmp.v_r = odom->vector.y;
odom_data.push_back(tmp);
}
// geometry_msgs::Vector3StampedConstPtr odom = m.instantiate<geometry_msgs::Vector3Stamped>();
// if (odom != NULL) {
// odometerData tmp;
// tmp.timestamp = odom->header.stamp;
// tmp.v_l = odom->vector.x;
// tmp.v_r = odom->vector.y;
// odom_data.push_back(tmp);
// }
}

// std::cout << "laser size: " << laser_data.size() << '\n' << "odom size: " << odom_data.size() << std::endl;
Expand Down
1 change: 1 addition & 0 deletions src/scan_match.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,7 @@ void cScanMatch::match(const std::vector<messageIO::laserScanData> &scan,
std::cout.width(3);
std::cout << progress << "%";
std::cout << "\b\b\b\b";
std::cout << std::flush;
}

std::cout << '\n' << std::endl;
Expand Down
22 changes: 10 additions & 12 deletions src/solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,8 +94,8 @@ void cSolver::calib(std::vector<cSynchronizer::sync_data> &calib_data, int outli

/* Now compute the FIM */
// 论文公式 9 误差的协方差
// std::cout <<'\n' << "Noise: " << '\n' << laser_std_x << ' ' << laser_std_y
// << ' ' << laser_std_th << std::endl;
// std::cout <<'\n' << "Noise: " << '\n' << laser_std_x << ' ' << laser_std_y
// << ' ' << laser_std_th << std::endl;

Eigen::Matrix3d laser_fim = Eigen::Matrix3d::Zero();
laser_fim(0,0) = (float)1 / (laser_std_x * laser_std_x);
Expand Down Expand Up @@ -123,21 +123,20 @@ void cSolver::calib(std::vector<cSynchronizer::sync_data> &calib_data, int outli
std::cout << "Uncertainty LiDAR-odom-yaw : "<< rad2deg( sqrt(state_cov(5,5)) )<<" deg \n";

return;

}

bool cSolver::solve(const std::vector<cSynchronizer::sync_data> &calib_data,
int mode, double max_cond_number, calib_result &res)
{
/*!<!--#################### FIRST STEP: estimate J21 and J22 #################-->*/
/*!<!--#################### FIRST STEP: estimate J21 and J22 #################-->*/
double J21, J22;

Eigen::Matrix2d A = Eigen::Matrix2d::Zero();
Eigen::Vector2d g = Eigen::Vector2d::Zero();
Eigen::Vector2d L_i = Eigen::Vector2d::Zero();

// std::cout << "A: " << A << ' ' << "g: " << g << std::endl;
// std::cout << "orz.." << std::endl;
// std::cout << "A: " << A << ' ' << "g: " << g << std::endl;
// std::cout << "orz.." << std::endl;

double th_i;
int n = (int)calib_data.size();
Expand All @@ -146,11 +145,11 @@ bool cSolver::solve(const std::vector<cSynchronizer::sync_data> &calib_data,
const cSynchronizer::sync_data &t = calib_data[i];
L_i(0) = t.T * t.velocity_left;
L_i(1) = t.T * t.velocity_right;
// std::cout << (L_i * L_i.transpose()) << '\n' << std::endl;
// std::cout << (L_i * L_i.transpose()) << '\n' << std::endl;
A = A + (L_i * L_i.transpose()); // A = A + L_i'*L_i; A symmetric
// std::cout << (t.scan_match_results[2] * L_i) << std::endl;
g = (t.scan_match_results[2] * L_i) + g; // g = g + L_i'*y_i; sm :{x , y, theta}
// std::cout << "A: " << A << ' ' << "g: " << g << std::endl;
// std::cout << (t.scan_match_results[2] * L_i) << std::endl;
g = (t.scan_match_results[2] * L_i) + g; // g = g + L_i'*L_i; sm :{x , y, theta}
// std::cout << "A: " << A << ' ' << "g: " << g << std::endl;
}

// Verify that A isn't singular
Expand All @@ -176,7 +175,7 @@ bool cSolver::solve(const std::vector<cSynchronizer::sync_data> &calib_data,
return 0;
}

/*!<!--############## SECOND STEP: estimate the remaining parameters ########-->*/
/*!<!--############## SECOND STEP: estimate the remaining parameters ########-->*/
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(5, 5);
Eigen::MatrixXd M2 = Eigen::MatrixXd::Zero(6, 6);
Eigen::MatrixXd L_k = Eigen::MatrixXd::Zero(2, 5);
Expand Down Expand Up @@ -356,7 +355,6 @@ Eigen::VectorXd cSolver::x_given_lambda(const Eigen::MatrixXd &M, const double &
Eigen::Vector2d tmp_v = Eigen::Vector2d::Zero(2);
tmp_v(0) = v0(3);
tmp_v(1) = v0(4);

double norm = tmp_v.norm();
double coeff = (v0(0) >= 0 ? 1 : -1) / norm;
v0 = coeff * v0;
Expand Down
33 changes: 17 additions & 16 deletions src/synchronizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,18 +39,7 @@ void cSynchronizer::synchronizeLaserOdom(const std::vector<messageIO::odometerDa
start_v_2_l = odom_data[j+1].v_l;
start_v_2_r = odom_data[j+1].v_r;
odom_it = j;
continue;
}
if (odom_data[j].timestamp.toSec() == csm_results[i].start_t.toSec())
{
start_t_1 = odom_data[j].timestamp;
start_t_2 = odom_data[j].timestamp;
start_v_1_l = odom_data[j].v_l;
start_v_1_r = odom_data[j].v_r;
start_v_2_l = odom_data[j].v_l;
start_v_2_r = odom_data[j].v_r;
odom_it = j;
continue;
// continue;
}
if ((odom_data[j].timestamp.toSec() < csm_results[i].end_t.toSec()) &&
(odom_data[j+1].timestamp.toSec() > csm_results[i].end_t.toSec()))
Expand All @@ -62,6 +51,17 @@ void cSynchronizer::synchronizeLaserOdom(const std::vector<messageIO::odometerDa
end_v_2_l = odom_data[j+1].v_l;
end_v_2_r = odom_data[j+1].v_r;
odom_it = j;
// continue;
}
if (odom_data[j].timestamp.toSec() == csm_results[i].start_t.toSec())
{
start_t_1 = odom_data[j].timestamp;
start_t_2 = odom_data[j].timestamp;
start_v_1_l = odom_data[j].v_l;
start_v_1_r = odom_data[j].v_r;
start_v_2_l = odom_data[j].v_l;
start_v_2_r = odom_data[j].v_r;
odom_it = j;
continue;
}
if (odom_data[j].timestamp.toSec() == csm_results[i].end_t.toSec())
Expand All @@ -80,14 +80,14 @@ void cSynchronizer::synchronizeLaserOdom(const std::vector<messageIO::odometerDa
float alpha = (csm_results[i].start_t.toSec() - start_t_1.toSec()) /
(start_t_2.toSec() - start_t_1.toSec());

double velocity_start_l = alpha * start_v_1_l + (1 - alpha) * start_v_2_l;
double velocity_start_r = alpha * start_v_1_r + (1 - alpha) * start_v_2_r;
double velocity_start_l = (1 - alpha) * start_v_1_l + alpha * start_v_2_l;
double velocity_start_r = (1 - alpha) * start_v_1_r + alpha * start_v_2_r;

float beta = (csm_results[i].end_t.toSec() - end_t_1.toSec()) /
(end_t_2.toSec() - end_t_1.toSec());

double velocity_end_l = beta * end_v_1_l + (1 - beta) * end_v_2_l;
double velocity_end_r = beta * end_v_1_r + (1 - beta) * end_v_2_r;
double velocity_end_l = (1 - beta) * end_v_1_l + beta * end_v_2_l;
double velocity_end_r = (1 - beta) * end_v_1_r + beta * end_v_2_r;

sync_data single_sync_data;
single_sync_data.T = csm_results[i].T;
Expand Down Expand Up @@ -115,6 +115,7 @@ void cSynchronizer::synchronizeLaserOdom(const std::vector<messageIO::odometerDa
std::cout.width(3);
std::cout << progress << "%";
std::cout << "\b\b\b\b";
std::cout << std::flush;
}
std::cout << '\n' << std::endl;

Expand Down