void main(int argc, char** argv){
node *head = NULL;
node *test = NULL;
node *merged = NULL;
addNode(5, &head);
addNode(6, &head);
addNode(7, &head);
addNode(10, &head);
addNode(4, &test);
addNode(8, &test);
addNode(11, &test);
merged = merge_sorted(head, test);
printList(merged);
//head->next->next->next->next = head->next;
if(detectLoop(head)){
printf("loop found\n");
exit(0);
}
// printList(head);
deleteNode(7, head);
// printList(head);
head = reverseList(head, NULL);
//printList(head);
destroyList(head);
}
int main(int argc, char * argv[]) {
node_t node = new nodeList();
node->value = 0;
node_t node1 = new nodeList();
node1->value = 1;
node->next = node1;
node_t node2 = new nodeList();
node2->value = 2;
node1->next = node2;
node_t node3 = new nodeList();
node3->value = 3;
node2->next = node3;
node3->next = node;
std::cout << detectLoop(node) << std::endl;
return detectLoop(node);
}
int main(int argc, char **argv)
{
struct node *head = NULL;
push(&head, 20);
push(&head, 4);
push(&head, 15);
push(&head, 10);
head->next->next->next->next = head;
detectLoop(head);
return 0;
}
int main(){
std::forward_list<int> l = {1,2,1};
auto i = l.begin();
std::next(i, 3) = i; //Equivalent to a->next n times
for(auto i = l.begin(); i != l.end(); ++i)
std::cout<<*i<<"\n";
std::cout<<"Loop "<<detectLoop(l)<<"\n";
return 0;
}
LinkNode *detectCycle(LinkNode *head){
LinkNode *meet_point = detectLoop(head);
LinkNode *start_head = NULL; //node start from head
LinkNode *start_meet = NULL; //node start from meet point
if(meet_point == NULL)
return NULL;
else{
start_head = head;
start_meet = meet_point;
while (start_head && start_meet && start_head != start_meet){
start_head = start_head->next;
start_meet = start_meet->next;
}
}
return start_head;
}
int cycleStartNode(struct listnode* head) {// time complexity O(n)
struct listnode* fast = head;
struct listnode* slow = head;
if (detectLoop(head)) {
while(slow && fast) {
fast = fast->next;
if(fast == slow) break;
fast = fast->next;
if(fast == slow) break;
slow = slow->next;
}
// at this point fast and slow both are pointing at the same node
fast = head;
while(slow != head) {
slow = slow->next;
fast = fast->next;
}
return slow;
}
return NULL;
}
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
if (sequence_cnt != cur_kf->sequence)
{
sequence_cnt++;
sequence_loop.push_back(0);
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
m_drift.lock();
t_drift = Eigen::Vector3d(0, 0, 0);
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;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;
global_index++;
int loop_index = -1;
if (flag_detect_loop)
{
TicToc tmp_t;
loop_index = detectLoop(cur_kf, cur_kf->index);
}
else
{
addKeyFrameIntoVoc(cur_kf);
}
if (loop_index != -1)
{
//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
KeyFrame* old_kf = getKeyFrame(loop_index);
if (cur_kf->findConnection(old_kf))
{
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
old_kf->getVioPose(w_P_old, w_R_old);
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
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 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++)
{
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->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;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
m_keyframelist.lock();
Vector3d P;
Matrix3d R;
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
cur_kf->updatePose(P, R);
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
//draw local connection
if (SHOW_S_EDGE)
{
list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
for (int i = 0; i < 4; i++)
{
if (rit == keyframelist.rend())
break;
Vector3d conncected_P;
Matrix3d connected_R;
if((*rit)->sequence == cur_kf->sequence)
{
(*rit)->getPose(conncected_P, connected_R);
posegraph_visualization->add_edge(P, conncected_P);
}
rit++;
}
}
if (SHOW_L_EDGE)
{
if (cur_kf->has_loop)
{
//printf("has loop \n");
KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
Vector3d connected_P,P0;
Matrix3d connected_R,R0;
connected_KF->getPose(connected_P, connected_R);
//cur_kf->getVioPose(P0, R0);
cur_kf->getPose(P0, R0);
if(cur_kf->sequence > 0)
{
//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);
keyframelist.push_back(cur_kf);
publish();
m_keyframelist.unlock();
}
void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
cur_kf->index = global_index;
global_index++;
int loop_index = -1;
if (flag_detect_loop)
loop_index = detectLoop(cur_kf, cur_kf->index);
else
{
addKeyFrameIntoVoc(cur_kf);
}
if (loop_index != -1)
{
printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
KeyFrame* old_kf = getKeyFrame(loop_index);
if (cur_kf->findConnection(old_kf))
{
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
m_keyframelist.lock();
Vector3d P;
Matrix3d R;
cur_kf->getPose(P, R);
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
base_path.poses.push_back(pose_stamped);
base_path.header = pose_stamped.header;
//draw local connection
if (SHOW_S_EDGE)
{
list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
for (int i = 0; i < 1; i++)
{
if (rit == keyframelist.rend())
break;
Vector3d conncected_P;
Matrix3d connected_R;
if((*rit)->sequence == cur_kf->sequence)
{
(*rit)->getPose(conncected_P, connected_R);
posegraph_visualization->add_edge(P, conncected_P);
}
rit++;
}
}
/*
if (cur_kf->has_loop)
{
KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
Vector3d connected_P;
Matrix3d connected_R;
connected_KF->getPose(connected_P, connected_R);
posegraph_visualization->add_loopedge(P, connected_P, SHIFT);
}
*/
keyframelist.push_back(cur_kf);
//publish();
m_keyframelist.unlock();
}
