Pose6d Pose6d::getInverse() const { double newx,newy,newz; tf::Matrix3x3 R = getBasis(); newx =-( R[0][0] * x + R[1][0] * y + R[2][0] * z); newy = -(R[0][1] * x + R[1][1] * y + R[2][1] * z); newz = -(R[0][2] * x + R[1][2] * y + R[2][2] * z); Pose6d new_pose(newx, newy, newz, -ax, -ay, -az); return(new_pose); }
void ProcessImage(std::vector<cv::Mat>& images) { #ifdef CHECK_NANS _MM_SET_EXCEPTION_MASK( _MM_GET_EXCEPTION_MASK() & ~(_MM_MASK_INVALID | _MM_MASK_OVERFLOW | _MM_MASK_DIV_ZERO)); #endif frame_count++; // if (poses.size() > 100) { // exit(EXIT_SUCCESS); // } Sophus::SE3d guess; // If this is a keyframe, set it as one on the tracker. prev_delta_t_ba = tracker.t_ba() * prev_t_ba.inverse(); if (is_prev_keyframe) { prev_t_ba = Sophus::SE3d(); } else { prev_t_ba = tracker.t_ba(); } // Add a pose to the poses array if (is_prev_keyframe) { std::shared_ptr<sdtrack::TrackerPose> new_pose(new sdtrack::TrackerPose); if (poses.size() > 0) { new_pose->t_wp = poses.back()->t_wp * last_t_ba.inverse(); } poses.push_back(new_pose); axes.push_back( std::unique_ptr<SceneGraph::GLAxis>(new SceneGraph::GLAxis(0.05))); gui_vars.scene_graph.AddChild(axes.back().get()); } guess = prev_delta_t_ba * prev_t_ba; if (guess.translation() == Eigen::Vector3d(0, 0, 0) && poses.size() > 1) { guess.translation() = Eigen::Vector3d(0, 0, 0.01); } tracker.AddImage(images, guess); tracker.EvaluateTrackResiduals(0, tracker.GetImagePyramid(), tracker.GetCurrentTracks()); if (!is_manual_mode) { tracker.OptimizeTracks(-1, optimize_landmarks, optimize_pose); tracker.PruneTracks(); } // Update the pose t_ab based on the result from the tracker. UpdateCurrentPose(); if (do_keyframing) { const double track_ratio = (double)tracker.num_successful_tracks() / (double)keyframe_tracks; const double total_trans = tracker.t_ba().translation().norm(); const double total_rot = tracker.t_ba().so3().log().norm(); bool keyframe_condition = track_ratio < 0.8 || total_trans > 0.2 || total_rot > 0.1; std::cerr << "\tRatio: " << track_ratio << " trans: " << total_trans << " rot: " << total_rot << std::endl; if (keyframe_tracks != 0) { if (keyframe_condition) { is_keyframe = true; } else { is_keyframe = false; } } // If this is a keyframe, set it as one on the tracker. prev_delta_t_ba = tracker.t_ba() * prev_t_ba.inverse(); if (is_keyframe) { tracker.AddKeyframe(); } is_prev_keyframe = is_keyframe; } else { tracker.AddKeyframe(); } std::cerr << "Num successful : " << tracker.num_successful_tracks() << " keyframe tracks: " << keyframe_tracks << std::endl; if (!is_manual_mode) { BaAndStartNewLandmarks(); } if (is_keyframe) { std::cerr << "KEYFRAME." << std::endl; keyframe_tracks = tracker.GetCurrentTracks().size(); std::cerr << "New keyframe tracks: " << keyframe_tracks << std::endl; } else { std::cerr << "NOT KEYFRAME." << std::endl; } current_tracks = &tracker.GetCurrentTracks(); #ifdef CHECK_NANS _MM_SET_EXCEPTION_MASK( _MM_GET_EXCEPTION_MASK() | (_MM_MASK_INVALID | _MM_MASK_OVERFLOW | _MM_MASK_DIV_ZERO)); #endif std::cerr << "FRAME : " << frame_count << " KEYFRAME: " << poses.size() << std::endl; }