diff --git a/camera_model/CMakeLists.txt b/camera_model/CMakeLists.txt index 281b14b..9c2f4f0 100644 --- a/camera_model/CMakeLists.txt +++ b/camera_model/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED COMPONENTS filesystem program_options system) include_directories(${Boost_INCLUDE_DIRS}) -find_package(OpenCV 3.4.5 REQUIRED) +find_package(OpenCV REQUIRED) # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") find_package(Ceres REQUIRED) @@ -34,7 +34,7 @@ include_directories( include_directories("include") -add_executable(Calibration +add_executable(Calibration src/intrinsic_calib.cc src/chessboard/Chessboard.cc src/calib/CameraCalibration.cc diff --git a/camera_model/src/calib/CameraCalibration.cc b/camera_model/src/calib/CameraCalibration.cc index 10543e9..682fe07 100644 --- a/camera_model/src/calib/CameraCalibration.cc +++ b/camera_model/src/calib/CameraCalibration.cc @@ -232,7 +232,7 @@ CameraCalibration::drawResults(std::vector& images) const cv::Mat& image = images.at(i); if (image.channels() == 1) { - cv::cvtColor(image, image, CV_GRAY2RGB); + cv::cvtColor(image, image, cv::COLOR_GRAY2RGB); } std::vector estImagePoints; @@ -250,12 +250,12 @@ CameraCalibration::drawResults(std::vector& images) const cv::circle(image, cv::Point(cvRound(pObs.x * drawMultiplier), cvRound(pObs.y * drawMultiplier)), - 5, green, 2, CV_AA, drawShiftBits); + 5, green, 2, cv::LINE_AA, drawShiftBits); cv::circle(image, cv::Point(cvRound(pEst.x * drawMultiplier), cvRound(pEst.y * drawMultiplier)), - 5, red, 2, CV_AA, drawShiftBits); + 5, red, 2, cv::LINE_AA, drawShiftBits); float error = cv::norm(pObs - pEst); @@ -272,7 +272,7 @@ CameraCalibration::drawResults(std::vector& images) const cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), - 1, CV_AA); + 1, cv::LINE_AA); } } diff --git a/camera_model/src/chessboard/Chessboard.cc b/camera_model/src/chessboard/Chessboard.cc index 4c0a761..097d9d7 100644 --- a/camera_model/src/chessboard/Chessboard.cc +++ b/camera_model/src/chessboard/Chessboard.cc @@ -17,13 +17,13 @@ Chessboard::Chessboard(cv::Size boardSize, cv::Mat& image) { if (image.channels() == 1) { - cv::cvtColor(image, mSketch, CV_GRAY2BGR); + cv::cvtColor(image, mSketch, cv::COLOR_GRAY2BGR); image.copyTo(mImage); } else { image.copyTo(mSketch); - cv::cvtColor(image, mImage, CV_BGR2GRAY); + cv::cvtColor(image, mImage, cv::COLOR_BGR2GRAY); } } @@ -31,10 +31,10 @@ void Chessboard::findCorners(bool useOpenCV) { mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners, - CV_CALIB_CB_ADAPTIVE_THRESH + - CV_CALIB_CB_NORMALIZE_IMAGE + - CV_CALIB_CB_FILTER_QUADS + - CV_CALIB_CB_FAST_CHECK, + cv::CALIB_CB_ADAPTIVE_THRESH + + cv::CALIB_CB_NORMALIZE_IMAGE + + cv::CALIB_CB_FILTER_QUADS + + cv::CALIB_CB_FAST_CHECK, useOpenCV); if (mCornersFound) @@ -141,24 +141,24 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, // Image histogram normalization and // BGR to Grayscale image conversion (if applicable) // MARTIN: Set to "false" - if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) + if (image.channels() != 1 || (flags & cv::CALIB_CB_NORMALIZE_IMAGE)) { cv::Mat norm_img(image.rows, image.cols, CV_8UC1); if (image.channels() != 1) { - cv::cvtColor(image, norm_img, CV_BGR2GRAY); + cv::cvtColor(image, norm_img, cv::COLOR_BGR2GRAY); img = norm_img; } - if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) + if (flags & cv::CALIB_CB_NORMALIZE_IMAGE) { cv::equalizeHist(image, norm_img); img = norm_img; } } - if (flags & CV_CALIB_CB_FAST_CHECK) + if (flags & cv::CALIB_CB_FAST_CHECK) { if (!checkChessboard(img, patternSize)) { @@ -189,13 +189,13 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, cv::Mat thresh_img; // convert the input grayscale image to binary (black-n-white) - if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) + if (flags & cv::CALIB_CB_ADAPTIVE_THRESH) { int blockSize = lround(prevSqrSize == 0 ? std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1; // convert to binary - cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5); + cv::adaptiveThreshold(img, thresh_img, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, blockSize, (k/2)*5); } else { @@ -204,7 +204,7 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, int thresh_level = lround(mean - 10); thresh_level = std::max(thresh_level, 10); - cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY); + cv::threshold(img, thresh_img, thresh_level, 255, cv::THRESH_BINARY); } // MARTIN's Code @@ -212,8 +212,8 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, // homogeneous dilation is performed, which is crucial for small, // distorted checkers. Use the CROSS kernel first, since its action // on the image is more subtle - cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1)); - cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1)); + cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3,3), cv::Point(1,1)); + cv::Mat kernel2 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3), cv::Point(1,1)); if (dilations >= 1) cv::dilate(thresh_img, thresh_img, kernel1); @@ -317,7 +317,7 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, } cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1), - cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1)); return true; } @@ -1172,7 +1172,7 @@ Chessboard::generateQuads(std::vector& quads, std::vector hierarchy; // Initialize contour retrieving routine - cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + cv::findContours(image, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); std::vector< std::vector > quadContours; @@ -1238,7 +1238,7 @@ Chessboard::generateQuads(std::vector& quads, dp = pt[1] - pt[2]; double d4 = sqrt(dp.dot(dp)); - if (!(flags & CV_CALIB_CB_FILTER_QUADS) || + if (!(flags & cv::CALIB_CB_FILTER_QUADS) || (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize && d1 >= 0.15 * p && d2 >= 0.15 * p)) { @@ -1583,20 +1583,20 @@ Chessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const bool result = false; for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f) { - cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY); + cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, cv::THRESH_BINARY); std::vector< std::vector > contours; std::vector hierarchy; // Initialize contour retrieving routine - cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); std::vector > quads; getQuadrangleHypotheses(contours, quads, 1); - cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV); + cv::threshold(black, thresh, threshLevel, 255, cv::THRESH_BINARY_INV); - cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + cv::findContours(thresh, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); getQuadrangleHypotheses(contours, quads, 0); const size_t min_quads_count = patternSize.width * patternSize.height / 2; diff --git a/camera_model/src/intrinsic_calib.cc b/camera_model/src/intrinsic_calib.cc index 5415d2c..846a999 100644 --- a/camera_model/src/intrinsic_calib.cc +++ b/camera_model/src/intrinsic_calib.cc @@ -237,7 +237,7 @@ int main(int argc, char** argv) { cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10,20), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), - 1, CV_AA); + 1, cv::LINE_AA); cv::imshow("Image", cbImages.at(i)); cv::waitKey(0); } diff --git a/feature_tracker/src/feature_tracker_node.cpp b/feature_tracker/src/feature_tracker_node.cpp index e29b084..2f6c31a 100644 --- a/feature_tracker/src/feature_tracker_node.cpp +++ b/feature_tracker/src/feature_tracker_node.cpp @@ -42,7 +42,7 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { // 一些常规的reset操作 ROS_WARN("image discontinue! reset the feature tracker!"); - first_image_flag = true; + first_image_flag = true; last_image_time = 0; pub_count = 1; std_msgs::Bool restart_flag; @@ -184,7 +184,7 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) for (int i = 0; i < NUM_OF_CAM; i++) { cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW); - cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); + cv::cvtColor(show_img, tmp_img, cv::COLOR_GRAY2RGB); for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++) { diff --git a/pose_graph/src/ThirdParty/DVision/BRIEF.cpp b/pose_graph/src/ThirdParty/DVision/BRIEF.cpp index 6af73c4..a2d0d64 100644 --- a/pose_graph/src/ThirdParty/DVision/BRIEF.cpp +++ b/pose_graph/src/ThirdParty/DVision/BRIEF.cpp @@ -2,8 +2,8 @@ * File: BRIEF.cpp * Author: Dorian Galvez * Date: September 2010 - * Description: implementation of BRIEF (Binary Robust Independent - * Elementary Features) descriptor by + * Description: implementation of BRIEF (Binary Robust Independent + * Elementary Features) descriptor by * Michael Calonder, Vincent Lepetitand Pascal Fua * + close binary tests (by Dorian Galvez-Lopez) * License: see the LICENSE.txt file @@ -36,21 +36,21 @@ BRIEF::~BRIEF() // --------------------------------------------------------------------------- -void BRIEF::compute(const cv::Mat &image, +void BRIEF::compute(const cv::Mat &image, const std::vector &points, vector &descriptors, bool treat_image) const { const float sigma = 2.f; const cv::Size ksize(9, 9); - + cv::Mat im; if(treat_image) { cv::Mat aux; if(image.depth() == 3) { - cv::cvtColor(image, aux, CV_RGB2GRAY); + cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY); } else { @@ -58,27 +58,27 @@ void BRIEF::compute(const cv::Mat &image, } cv::GaussianBlur(aux, im, ksize, sigma, sigma); - + } else { im = image; } - + assert(im.type() == CV_8UC1); assert(im.isContinuous()); - + // use im now const int W = im.cols; const int H = im.rows; - + descriptors.resize(points.size()); std::vector::iterator dit; std::vector::const_iterator kit; - + int x1, y1, x2, y2; - + dit = descriptors.begin(); for(kit = points.begin(); kit != points.end(); ++kit, ++dit) { @@ -91,16 +91,16 @@ void BRIEF::compute(const cv::Mat &image, y1 = (int)(kit->pt.y + m_y1[i]); x2 = (int)(kit->pt.x + m_x2[i]); y2 = (int)(kit->pt.y + m_y2[i]); - - if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H + + if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H) { if( im.ptr(y1)[x1] < im.ptr(y2)[x2] ) { dit->set(i); - } + } } // if (x,y)_1 and (x,y)_2 are in the image - + } // for each (x,y) } // for each keypoint } @@ -108,7 +108,7 @@ void BRIEF::compute(const cv::Mat &image, // --------------------------------------------------------------------------- void BRIEF::generateTestPoints() -{ +{ m_x1.resize(m_bit_length); m_y1.resize(m_bit_length); m_x2.resize(m_bit_length); @@ -117,31 +117,31 @@ void BRIEF::generateTestPoints() const float g_mean = 0.f; const float g_sigma = 0.2f * (float)m_patch_size; const float c_sigma = 0.08f * (float)m_patch_size; - + float sigma2; if(m_type == RANDOM) sigma2 = g_sigma; else sigma2 = c_sigma; - + const int max_v = m_patch_size / 2; - + DUtils::Random::SeedRandOnce(); - + for(int i = 0; i < m_bit_length; ++i) { int x1, y1, x2, y2; - + do { x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); } while( x1 > max_v || x1 < -max_v); - + do { y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); } while( y1 > max_v || y1 < -max_v); - + float meanx, meany; if(m_type == RANDOM) meanx = meany = g_mean; @@ -150,17 +150,17 @@ void BRIEF::generateTestPoints() meanx = x1; meany = y1; } - + do { x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2); } while( x2 > max_v || x2 < -max_v); - + do { y2 = DUtils::Random::RandomGaussianValue(meany, sigma2); } while( y2 > max_v || y2 < -max_v); - + m_x1[i] = x1; m_y1[i] = y1; m_x2[i] = x2; @@ -170,5 +170,3 @@ void BRIEF::generateTestPoints() } // ---------------------------------------------------------------------------- - - diff --git a/pose_graph/src/keyframe.cpp b/pose_graph/src/keyframe.cpp index ee9cf97..f0510a6 100644 --- a/pose_graph/src/keyframe.cpp +++ b/pose_graph/src/keyframe.cpp @@ -13,11 +13,11 @@ static void reduceVector(vector &v, vector status) // create keyframe online /** * @brief Construct a new Key Frame:: Key Frame object，创建一个KF对象，计算已有特征点的描述子，同时额外提取fast角点并计算描述子 - * + * * @param[in] _time_stamp KF的时间戳 * @param[in] _index KF的索引 * @param[in] _vio_T_w_i vio节点中的位姿 - * @param[in] _vio_R_w_i + * @param[in] _vio_R_w_i * @param[in] _image 对应的原图 * @param[in] _point_3d KF对应VIO节点中的世界坐标 * @param[in] _point_2d_uv 像素坐标 @@ -35,7 +35,7 @@ KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3 vio_R_w_i = _vio_R_w_i; T_w_i = vio_T_w_i; R_w_i = vio_R_w_i; - origin_vio_T = vio_T_w_i; + origin_vio_T = vio_T_w_i; origin_vio_R = vio_R_w_i; image = _image.clone(); cv::resize(image, thumbnail, cv::Size(80, 60)); // 这个缩小尺寸应该是为了可视化 @@ -57,19 +57,19 @@ KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3 // load previous keyframe /** * @brief Construct a new Key Frame:: Key Frame object从已有的地图中加载KF，就是一些简单的赋值操作 - * - * @param[in] _time_stamp - * @param[in] _index - * @param[in] _vio_T_w_i - * @param[in] _vio_R_w_i - * @param[in] _T_w_i - * @param[in] _R_w_i - * @param[in] _image - * @param[in] _loop_index - * @param[in] _loop_info - * @param[in] _keypoints - * @param[in] _keypoints_norm - * @param[in] _brief_descriptors + * + * @param[in] _time_stamp + * @param[in] _index + * @param[in] _vio_T_w_i + * @param[in] _vio_R_w_i + * @param[in] _T_w_i + * @param[in] _R_w_i + * @param[in] _image + * @param[in] _loop_index + * @param[in] _loop_info + * @param[in] _keypoints + * @param[in] _keypoints_norm + * @param[in] _brief_descriptors */ KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, cv::Mat &_image, int _loop_index, Eigen::Matrix &_loop_info, @@ -103,7 +103,7 @@ KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3 /** * @brief 计算已有特征点的描述子 - * + * */ void KeyFrame::computeWindowBRIEFPoint() { @@ -154,15 +154,15 @@ void BriefExtractor::operator() (const cv::Mat &im, vector &keys, /** * @brief 暴力匹配法，通过遍历所有的候选描述子得到最佳匹配 - * + * * @param[in] window_descriptor 当前帧的一个描述子 * @param[in] descriptors_old 回环帧的描述子集合 * @param[in] keypoints_old 回环帧像素坐标集合 * @param[in] keypoints_old_norm 回环帧归一化坐标集合 * @param[out] best_match 最佳匹配的像素坐标 * @param[out] best_match_norm 最佳匹配的归一化相机坐标 - * @return true - * @return false + * @return true + * @return false */ bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor, const std::vector &descriptors_old, @@ -198,7 +198,7 @@ bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor, /** * @brief 将当前帧的描述子依次和回环帧描述子进行匹配，得到匹配结果 - * + * * @param[out] matched_2d_old 匹配回环帧点的像素坐标集合 * @param[out] matched_2d_old_norm 匹配回环帧点的归一化相机坐标集合 * @param[out] status 状态位 @@ -258,12 +258,12 @@ void KeyFrame::FundmantalMatrixRANSAC(const std::vector &matched_2d /** * @brief 通过PNP对当前帧和回环是否构成回环进行校验 - * + * * @param[in] matched_2d_old_norm 回环帧2d归一化坐标 * @param[in] matched_3d 当前帧3d地图点 - * @param[out] status - * @param[out] PnP_T_old - * @param[out] PnP_R_old + * @param[out] status + * @param[out] PnP_T_old + * @param[out] PnP_R_old */ void KeyFrame::PnPRANSAC(const vector &matched_2d_old_norm, const std::vector &matched_3d, @@ -327,10 +327,10 @@ void KeyFrame::PnPRANSAC(const vector &matched_2d_old_norm, /** * @brief 寻找两帧之间联系，确定是否回环 - * - * @param[in] old_kf - * @return true - * @return false + * + * @param[in] old_kf + * @return true + * @return false */ bool KeyFrame::findConnection(KeyFrame* old_kf) { @@ -349,12 +349,12 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) TicToc t_match; #if 0 - if (DEBUG_IMAGE) + if (DEBUG_IMAGE) { cv::Mat gray_img, loop_match_img; cv::Mat old_img = old_kf->image; cv::hconcat(image, old_img, gray_img); - cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); + cvtColor(gray_img, loop_match_img, cv::COLOR_GRAY2RGB); for(int i = 0; i< (int)point_2d_uv.size(); i++) { cv::Point2f cur_pt = point_2d_uv[i]; @@ -385,7 +385,7 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) reduceVector(matched_id, status); // 当前帧对应的地图点索引 //printf("search by des finish\n"); - #if 0 + #if 0 if (DEBUG_IMAGE) { int gap = 10; @@ -394,7 +394,7 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) cv::Mat old_img = old_kf->image; cv::hconcat(image, gap_image, gap_image); cv::hconcat(gap_image, old_img, gray_img); - cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); + cvtColor(gray_img, loop_match_img, cv::COLOR_GRAY2RGB); for(int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; @@ -426,9 +426,9 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) path2 << "/home/tony-ws1/raw_data/loop_image/" << index << "-" << old_kf->index << "-" << "1descriptor_match_2.jpg"; - cv::imwrite( path2.str().c_str(), old_img); + cv::imwrite( path2.str().c_str(), old_img); */ - + } #endif status.clear(); @@ -450,7 +450,7 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) cv::Mat old_img = old_kf->image; cv::hconcat(image, gap_image, gap_image); cv::hconcat(gap_image, old_img, gray_img); - cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); + cvtColor(gray_img, loop_match_img, cv::COLOR_GRAY2RGB); for(int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; @@ -503,7 +503,7 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) cv::Mat old_img = old_kf->image; cv::hconcat(image, gap_image, gap_image); cv::hconcat(gap_image, old_img, gray_img); - cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); + cvtColor(gray_img, loop_match_img, cv::COLOR_GRAY2RGB); for(int i = 0; i< (int)matched_2d_cur.size(); i++) { cv::Point2f cur_pt = matched_2d_cur[i]; @@ -522,9 +522,9 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 2, 8, 0); } cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255)); - putText(notation, "current frame: " + to_string(index) + " sequence: " + to_string(sequence), cv::Point2f(20, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); + putText(notation, "current frame: " + to_string(index) + " sequence: " + to_string(sequence), cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); - putText(notation, "previous frame: " + to_string(old_kf->index) + " sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + COL + gap, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); + putText(notation, "previous frame: " + to_string(old_kf->index) + " sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + COL + gap, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3); cv::vconcat(notation, loop_match_img, loop_match_img); /* @@ -537,8 +537,8 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) if ((int)matched_2d_cur.size() > MIN_LOOP_NUM) { /* - cv::imshow("loop connection",loop_match_img); - cv::waitKey(10); + cv::imshow("loop connection",loop_match_img); + cv::waitKey(10); */ cv::Mat thumbimage; cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2)); @@ -632,9 +632,9 @@ void KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d & /** * @brief 更新一些vio位姿，将回环修正的累计误差补偿进来 - * - * @param[in] _T_w_i - * @param[in] _R_w_i + * + * @param[in] _T_w_i + * @param[in] _R_w_i */ void KeyFrame::updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i) { @@ -687,5 +687,3 @@ BriefExtractor::BriefExtractor(const std::string &pattern_file) m_brief.importPairs(x1, y1, x2, y2); // dbow加载pattern用于后续计算描述子 } - - diff --git a/pose_graph/src/pose_graph.cpp b/pose_graph/src/pose_graph.cpp index 7b797e1..29a836e 100644 --- a/pose_graph/src/pose_graph.cpp +++ b/pose_graph/src/pose_graph.cpp @@ -28,8 +28,8 @@ PoseGraph::~PoseGraph() } /** * @brief 注册一些发布publisher - * - * @param[in] n + * + * @param[in] n */ void PoseGraph::registerPub(ros::NodeHandle &n) { @@ -107,16 +107,16 @@ 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; // 回环矫正前的位姿认为是T_w'_cur // 下面求得是 T_w_cur * T_cur_w' = T_w_w' shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0)); - 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 // 如果这两个不是同一个sequence，并且当前sequence没有跟之前合并 if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0) - { + { w_r_vio = shift_r; w_t_vio = shift_t; // T_w_w' * T_w'_cur @@ -125,7 +125,7 @@ void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) cur_kf->updateVioPose(vio_P_cur, vio_R_cur); // 更新当前帧位姿 list::iterator it = keyframelist.begin(); // 同时把这个序列当前帧之间的位姿都更新过来 - for (; it != keyframelist.end(); it++) + for (; it != keyframelist.end(); it++) { if((*it)->sequence == cur_kf->sequence) { @@ -218,7 +218,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); @@ -230,8 +230,8 @@ void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) /** * @brief 加载KF - * - * @param[in] cur_kf + * + * @param[in] cur_kf * @param[in] flag_detect_loop 是否做回环检测 */ void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) @@ -314,15 +314,15 @@ void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) /** * @brief 根据index找到对应的KF对象的指针 - * - * @param[in] index - * @return KeyFrame* + * + * @param[in] index + * @return KeyFrame* */ KeyFrame* PoseGraph::getKeyFrame(int index) { // unique_lock lock(m_keyframelist); list::iterator it = keyframelist.begin(); - for (; it != keyframelist.end(); it++) + for (; it != keyframelist.end(); it++) { if((*it)->index == index) break; @@ -342,7 +342,7 @@ int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); - putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[frame_index] = compressed_image; } TicToc tmp_t; @@ -365,9 +365,9 @@ int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index) { loop_result = compressed_image.clone(); 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)); + 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++) @@ -375,7 +375,7 @@ int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index) int tmp_index = ret[i].Id; auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); - putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); + putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } @@ -388,14 +388,14 @@ 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) { auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); - putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } @@ -432,7 +432,7 @@ void PoseGraph::addKeyFrameIntoVoc(KeyFrame* keyframe) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); - putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[keyframe->index] = compressed_image; } @@ -441,7 +441,7 @@ void PoseGraph::addKeyFrameIntoVoc(KeyFrame* keyframe) /** * @brief 如果找到回环，该线程就执行位姿优化 - * + * */ void PoseGraph::optimize4DoF() { @@ -517,7 +517,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]); } @@ -536,9 +536,9 @@ void PoseGraph::optimize4DoF() ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(), relative_yaw, euler_conncected.y(), euler_conncected.z()); // 对i-j帧和第i帧都成约束 - 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]); } } @@ -555,13 +555,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++; @@ -570,7 +570,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++) @@ -700,8 +700,8 @@ void PoseGraph::updatePath() { list::reverse_iterator rit = keyframelist.rbegin(); list::reverse_iterator lrit; - for (; rit != keyframelist.rend(); rit++) - { + for (; rit != keyframelist.rend(); rit++) + { if ((*rit)->index == (*it)->index) { lrit = rit; @@ -721,13 +721,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; @@ -748,7 +748,7 @@ void PoseGraph::updatePath() /** * @brief 保存视觉地图 - * + * */ void PoseGraph::savePoseGraph() { @@ -776,12 +776,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()); @@ -797,7 +797,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(); @@ -811,7 +811,7 @@ void PoseGraph::savePoseGraph() /** * @brief 加载已有的视觉地图 - * + * */ void PoseGraph::loadPoseGraph() { @@ -839,24 +839,24 @@ void PoseGraph::loadPoseGraph() Eigen::Matrix 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); */ @@ -893,7 +893,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"; @@ -977,11 +977,11 @@ void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix &_loo w_R_cur = w_R_old * relative_q; double shift_yaw; Matrix3d shift_r; - Vector3d shift_t; + Vector3d shift_t; // 更新VIO位姿和修正位姿的delta pose shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0)); - 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; m_drift.lock(); yaw_drift = shift_yaw;