Skip to content

Thread issue fixes, reset vins fixes #30

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

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Thread issue fixes, reset vins fixes #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
d2590e5
fixed threading issues and got the reset bit correctly working. TODO …
mpkuse Mar 20, 2019
30edb4d
minor fix in thread
mpkuse Mar 20, 2019
d71a352
added a publish function fake_publish. Implemented a hacky way to det…
mpkuse Mar 20, 2019
12d4fab
fix the image equalize for dealing correctly with dark images
mpkuse Mar 21, 2019
d8a6748
fixed the rosTest for recovering for failures, aka restart mechanism
mpkuse Mar 26, 2019
9f189a3
release flag
mpkuse Jun 5, 2019
7d707ef
feature tracking window size reduced slightly for performance boost i…
mpkuse Jun 6, 2019
4aad873
i have noticed tat freq config file argument is not used, so i manual…
mpkuse Jul 29, 2019
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
106 changes: 53 additions & 53 deletions loop_fusion/src/pose_graph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
*
* This file is part of VINS.
*
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
Expand All @@ -15,7 +15,7 @@ PoseGraph::PoseGraph()
{
posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0);
posegraph_visualization->setScale(0.1);
posegraph_visualization->setLineWidth(0.01);
posegraph_visualization->setLineWidth(0.05);
earliest_loop_index = -1;
t_drift = Eigen::Vector3d(0, 0, 0);
yaw_drift = 0;
Expand Down Expand Up @@ -81,7 +81,7 @@ void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
r_drift = Eigen::Matrix3d::Identity();
m_drift.unlock();
}

cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
Expand Down Expand Up @@ -121,25 +121,25 @@ void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
Vector3d shift_t;
if(use_imu)
{
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
}
else
shift_r = w_R_cur * vio_R_cur.transpose();
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
// shift vio pose of whole sequence to the world frame
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
{
w_r_vio = shift_r;
w_t_vio = shift_t;
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
for (; it != keyframelist.end(); it++)
{
if((*it)->sequence == cur_kf->sequence)
{
Expand Down Expand Up @@ -230,7 +230,7 @@ void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
//printf("add loop into visual \n");
posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
}

}
}
//posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);
Expand Down Expand Up @@ -321,7 +321,7 @@ KeyFrame* PoseGraph::getKeyFrame(int index)
{
// unique_lock<mutex> lock(m_keyframelist);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
for (; it != keyframelist.end(); it++)
{
if((*it)->index == index)
break;
Expand Down Expand Up @@ -363,7 +363,7 @@ int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
if (ret.size() > 0)
putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
}
// visual loop result
// visual loop result
if (DEBUG_IMAGE)
{
for (unsigned int i = 0; i < ret.size(); i++)
Expand All @@ -381,7 +381,7 @@ int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
//if (ret[i].Score > ret[0].Score * 0.3)
if (ret[i].Score > 0.015)
{
{
find_loop = true;
int tmp_index = ret[i].Id;
if (DEBUG_IMAGE && 0)
Expand Down Expand Up @@ -502,7 +502,7 @@ void PoseGraph::optimize4DoF()
problem.AddParameterBlock(t_array[i], 3);

if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
Expand All @@ -518,15 +518,15 @@ void PoseGraph::optimize4DoF()
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
}
}

//add loop edge

if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
Expand All @@ -537,13 +537,13 @@ void PoseGraph::optimize4DoF()
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
t_array[i]);

}

if ((*it)->index == cur_index)
break;
i++;
Expand All @@ -552,7 +552,7 @@ void PoseGraph::optimize4DoF()

ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";

//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
Expand Down Expand Up @@ -678,7 +678,7 @@ void PoseGraph::optimize6DoF()
problem.AddParameterBlock(t_array[i], 3);

if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
{
problem.SetParameterBlockConstant(q_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
Expand All @@ -701,7 +701,7 @@ void PoseGraph::optimize6DoF()
}

//add loop edge

if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
Expand All @@ -713,9 +713,9 @@ void PoseGraph::optimize6DoF()
ceres::CostFunction* loop_function = RelativeRTError::Create(relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
0.1, 0.01);
problem.AddResidualBlock(loop_function, loss_function, q_array[connected_index], t_array[connected_index], q_array[i], t_array[i]);
problem.AddResidualBlock(loop_function, loss_function, q_array[connected_index], t_array[connected_index], q_array[i], t_array[i]);
}

if ((*it)->index == cur_index)
break;
i++;
Expand All @@ -724,7 +724,7 @@ void PoseGraph::optimize6DoF()

ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";

//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
Expand Down Expand Up @@ -848,8 +848,8 @@ void PoseGraph::updatePath()
{
list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
list<KeyFrame*>::reverse_iterator lrit;
for (; rit != keyframelist.rend(); rit++)
{
for (; rit != keyframelist.rend(); rit++)
{
if ((*rit)->index == (*it)->index)
{
lrit = rit;
Expand All @@ -869,13 +869,13 @@ void PoseGraph::updatePath()
}
break;
}
}
}
}
if (SHOW_L_EDGE)
{
if ((*it)->has_loop && (*it)->sequence == sequence_cnt)
{

KeyFrame* connected_KF = getKeyFrame((*it)->loop_index);
Vector3d connected_P;
Matrix3d connected_R;
Expand Down Expand Up @@ -919,12 +919,12 @@ void PoseGraph::savePoseGraph()
Vector3d VIO_tmp_T = (*it)->vio_T_w_i;
Vector3d PG_tmp_T = (*it)->T_w_i;

fprintf (pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f %f %d\n",(*it)->index, (*it)->time_stamp,
VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(),
PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(),
VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(),
PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(),
(*it)->loop_index,
fprintf (pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f %f %d\n",(*it)->index, (*it)->time_stamp,
VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(),
PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(),
VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(),
PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(),
(*it)->loop_index,
(*it)->loop_info(0), (*it)->loop_info(1), (*it)->loop_info(2), (*it)->loop_info(3),
(*it)->loop_info(4), (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7),
(int)(*it)->keypoints.size());
Expand All @@ -939,7 +939,7 @@ void PoseGraph::savePoseGraph()
for (int i = 0; i < (int)(*it)->keypoints.size(); i++)
{
brief_file << (*it)->brief_descriptors[i] << endl;
fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y,
fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y,
(*it)->keypoints_norm[i].pt.x, (*it)->keypoints_norm[i].pt.y);
}
brief_file.close();
Expand Down Expand Up @@ -975,24 +975,24 @@ void PoseGraph::loadPoseGraph()
int keypoints_num;
Eigen::Matrix<double, 8, 1 > loop_info;
int cnt = 0;
while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp,
&VIO_Tx, &VIO_Ty, &VIO_Tz,
&PG_Tx, &PG_Ty, &PG_Tz,
&VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz,
&PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz,
while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp,
&VIO_Tx, &VIO_Ty, &VIO_Tz,
&PG_Tx, &PG_Ty, &PG_Tz,
&VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz,
&PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz,
&loop_index,
&loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3,
&loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3,
&loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7,
&keypoints_num) != EOF)
&keypoints_num) != EOF)
{
/*
printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp,
VIO_Tx, VIO_Ty, VIO_Tz,
PG_Tx, PG_Ty, PG_Tz,
VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz,
PG_Qw, PG_Qx, PG_Qy, PG_Qz,
printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp,
VIO_Tx, VIO_Ty, VIO_Tz,
PG_Tx, PG_Ty, PG_Tz,
VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz,
PG_Qw, PG_Qx, PG_Qy, PG_Qz,
loop_index,
loop_info_0, loop_info_1, loop_info_2, loop_info_3,
loop_info_0, loop_info_1, loop_info_2, loop_info_3,
loop_info_4, loop_info_5, loop_info_6, loop_info_7,
keypoints_num);
*/
Expand Down Expand Up @@ -1028,7 +1028,7 @@ void PoseGraph::loadPoseGraph()
earliest_loop_index = loop_index;
}

// load keypoints, brief_descriptors
// load keypoints, brief_descriptors
string brief_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_briefdes.dat";
std::ifstream brief_file(brief_path, std::ios::binary);
string keypoints_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_keypoints.txt";
Expand Down
26 changes: 14 additions & 12 deletions loop_fusion/src/utility/CameraPoseVisualization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
*
* This file is part of VINS.
*
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*******************************************************/
Expand Down Expand Up @@ -66,6 +66,7 @@ void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::V
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.01;
marker.scale.x = m_scale;

marker.color.b = 1.0f;
marker.color.a = 1.0;
Expand Down Expand Up @@ -95,9 +96,10 @@ void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eige
marker.lifetime = ros::Duration();
//marker.scale.x = 0.4;
marker.scale.x = 0.02;
marker.scale.x = m_scale;
marker.color.r = 1.0f;
//marker.color.g = 1.0f;
//marker.color.b = 1.0f;
marker.color.g = 0.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;

geometry_msgs::Point point0, point1;
Expand Down Expand Up @@ -216,12 +218,12 @@ void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::H
}
*/


for(auto& marker : m_markers) {
marker.header = header;
markerArray_msg.markers.push_back(marker);
}

pub.publish(markerArray_msg);
}

Expand Down Expand Up @@ -264,7 +266,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M
Eigen::Vector3d p_cam, p_w;
p_cam.z() = 0;
p_cam.x() = (r - center_x) * scale;
p_cam.y() = (c - center_y) * scale;
p_cam.y() = (c - center_y) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand All @@ -274,7 +276,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M

p_cam.z() = 0;
p_cam.x() = (r - center_x + 1) * scale;
p_cam.y() = (c - center_y) * scale;
p_cam.y() = (c - center_y) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand All @@ -284,7 +286,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M

p_cam.z() = 0;
p_cam.x() = (r - center_x) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand All @@ -294,7 +296,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M

p_cam.z() = 0;
p_cam.x() = (r - center_x + 1) * scale;
p_cam.y() = (c - center_y) * scale;
p_cam.y() = (c - center_y) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand All @@ -304,7 +306,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M

p_cam.z() = 0;
p_cam.x() = (r - center_x + 1) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand All @@ -314,7 +316,7 @@ void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::M

p_cam.z() = 0;
p_cam.x() = (r - center_x) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_cam.y() = (c - center_y + 1) * scale;
p_w = R * p_cam + T;
p.x = p_w(0);
p.y = p_w(1);
Expand Down
Loading