From ece2a404e5581ff94c309dc0ff80f6dd298f5a1e Mon Sep 17 00:00:00 2001 From: xuhao3e8 Date: Wed, 23 Oct 2024 19:50:36 +0800 Subject: [PATCH] Fix some of warnings... --- camera_models/CMakeLists.txt | 2 +- d2common/include/d2common/d2frontend_types.h | 8 +++---- d2common/include/d2common/d2landmarks.h | 3 ++- d2common/include/d2common/fisheye_undistort.h | 2 +- d2common/include/d2common/integration_base.h | 2 +- d2common/src/d2vinsframe.cpp | 2 +- d2common/src/solver/BaseParamResInfo.cpp | 4 ++-- d2frontend/CMakeLists.txt | 2 +- d2frontend/include/d2frontend/utils.h | 2 +- d2frontend/src/CNN/superpoint.cpp | 10 ++++----- d2frontend/src/CNN/superpoint_common.cpp | 8 +++---- d2frontend/src/d2featuretracker.cpp | 22 +++++++++---------- d2frontend/src/loop_cam.cpp | 2 +- d2frontend/src/loop_detector.cpp | 2 +- d2frontend/src/loop_utils.cpp | 14 ++++++------ d2pgo/src/d2pgo.cpp | 2 +- .../rot_init/rotation_initialization_base.hpp | 2 +- .../fast_max-clique_finder/src/findClique.cpp | 4 ++-- d2vins/src/estimator/d2estimator.cpp | 2 +- d2vins/src/estimator/d2vinsstate.cpp | 14 ++++++------ d2vins/src/estimator/solver/ConsensusSync.cpp | 8 +++---- .../estimator/solver/VINSConsenusSolver.cpp | 4 ++-- d2vins/src/network/d2vins_net.cpp | 2 +- d2vins/src/utils/solve_5pts.cpp | 2 +- d2vins/src/visualization/visualization.cpp | 8 +++---- 25 files changed, 67 insertions(+), 66 deletions(-) diff --git a/camera_models/CMakeLists.txt b/camera_models/CMakeLists.txt index 0085a140..1e96e345 100644 --- a/camera_models/CMakeLists.txt +++ b/camera_models/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.0) project(camera_models) set(CMAKE_BUILD_TYPE "Release") diff --git a/d2common/include/d2common/d2frontend_types.h b/d2common/include/d2common/d2frontend_types.h index cbf92d51..90f8594a 100644 --- a/d2common/include/d2common/d2frontend_types.h +++ b/d2common/include/d2common/d2frontend_types.h @@ -217,7 +217,7 @@ struct VisualImageDesc { img_desc.landmark_descriptor_int8.resize(landmark_descriptor.size()); img_desc.landmark_descriptor_size = 0; auto max = desc0.cwiseAbs().maxCoeff(); - for (int i = 0; i < landmark_descriptor.size(); i++) { + for (unsigned int i = 0; i < landmark_descriptor.size(); i++) { img_desc.landmark_descriptor_int8[i] = (int8_t)(desc0[i] / max * 127); } img_desc.landmark_scores_size = 0; @@ -248,7 +248,7 @@ struct VisualImageDesc { img_desc.header.image_desc_int8.resize(image_desc.size()); img_desc.header.image_desc_size = 0; double max = Eigen::Map(image_desc.data(), image_desc.size()).cwiseAbs().maxCoeff(); - for (int i = 0; i < image_desc.size(); i++) { + for (unsigned int i = 0; i < image_desc.size(); i++) { img_desc.header.image_desc_int8[i] = (int8_t)(image_desc[i] / max * 127); } } else { @@ -304,7 +304,7 @@ struct VisualImageDesc { if (desc.landmark_descriptor_int8.size() > 0) { landmark_descriptor.resize(desc.landmark_descriptor_int8.size()); Eigen::Map desc0(landmark_descriptor.data(), landmark_descriptor.size()); - for (int i = 0; i < landmark_descriptor.size(); i++) { + for (unsigned int i = 0; i < landmark_descriptor.size(); i++) { desc0(i) = desc.landmark_descriptor_int8[i] / 127.0; } //Per feature normalize the landmark desc @@ -317,7 +317,7 @@ struct VisualImageDesc { if (desc.header.image_desc_size_int8 > 0) { image_desc.resize(desc.header.image_desc_size_int8); Eigen::Map gdesc(image_desc.data(), image_desc.size()); - for (int i = 0; i < image_desc.size(); i++) { + for (unsigned int i = 0; i < image_desc.size(); i++) { gdesc(i) = desc.header.image_desc_int8[i] / 127.0; } gdesc.normalize(); diff --git a/d2common/include/d2common/d2landmarks.h b/d2common/include/d2common/d2landmarks.h index 05da7f3c..3cc6efb6 100644 --- a/d2common/include/d2common/d2landmarks.h +++ b/d2common/include/d2common/d2landmarks.h @@ -5,6 +5,7 @@ #include #include "d2basetypes.h" #include +#include namespace D2Common { enum LandmarkFlag { @@ -268,7 +269,7 @@ struct LandmarkPerId { return it; } } - printf("LandmarkPerId::at: Error, cannot find frame_id %d\n", frame_id); + spdlog::warn("LandmarkPerId::at Warn, cannot find frame_id {}", frame_id); return LandmarkPerFrame(); } diff --git a/d2common/include/d2common/fisheye_undistort.h b/d2common/include/d2common/fisheye_undistort.h index 71c3f7ab..443a76e9 100644 --- a/d2common/include/d2common/fisheye_undistort.h +++ b/d2common/include/d2common/fisheye_undistort.h @@ -129,7 +129,7 @@ class FisheyeUndist { auto _photometics_gpu = undist_all_cuda(photomertic, true); photometics = _photometics; photometics_gpu = _photometics_gpu; - for (int i = 0; i < _photometics.size(); i++) { + for (unsigned int i = 0; i < _photometics.size(); i++) { cv::Mat bgr; cv::cvtColor(_photometics[i], bgr, cv::COLOR_GRAY2BGR); photometics_bgr.push_back(bgr); diff --git a/d2common/include/d2common/integration_base.h b/d2common/include/d2common/integration_base.h index 57ace0c2..358a98a0 100644 --- a/d2common/include/d2common/integration_base.h +++ b/d2common/include/d2common/integration_base.h @@ -88,7 +88,7 @@ class IntegrationBase linearized_bg = _linearized_bg; jacobian.setIdentity(); covariance.setZero(); - for (int i = 0; i < static_cast(dt_buf.size()); i++) + for (unsigned int i = 0; i < dt_buf.size(); i++) propagate(dt_buf[i], acc_buf[i], gyr_buf[i]); } diff --git a/d2common/src/d2vinsframe.cpp b/d2common/src/d2vinsframe.cpp index 1726a7e0..90affeef 100644 --- a/d2common/src/d2vinsframe.cpp +++ b/d2common/src/d2vinsframe.cpp @@ -99,7 +99,7 @@ swarm_msgs::VIOFrame VINSFrame::toROS(const std::vector& exts) { msg.is_keyframe = is_keyframe; msg.reference_frame_id = reference_frame_id; msg.odom = odom.toRos(); - for (int i = 0; i < exts.size(); i++) { + for (unsigned int i = 0; i < exts.size(); i++) { msg.extrinsics.emplace_back(exts[i].toROS()); } return msg; diff --git a/d2common/src/solver/BaseParamResInfo.cpp b/d2common/src/solver/BaseParamResInfo.cpp index 3ca1124d..30edc843 100644 --- a/d2common/src/solver/BaseParamResInfo.cpp +++ b/d2common/src/solver/BaseParamResInfo.cpp @@ -61,7 +61,7 @@ void ResidualInfo::Evaluate(const std::vector ¶m_infos, std::vector blk_sizes = cost_function->parameter_block_sizes(); std::vector raw_jacobians(blk_sizes.size()); jacobians.resize(blk_sizes.size()); - for (int i = 0; i < static_cast(blk_sizes.size()); i++) { + for (unsigned int i = 0; i < blk_sizes.size(); i++) { jacobians[i].resize(cost_function->num_residuals(), blk_sizes[i]); jacobians[i].setZero(); raw_jacobians[i] = jacobians[i].data(); @@ -83,7 +83,7 @@ void ResidualInfo::Evaluate(const std::vector ¶m_infos, residual_scaling_ = sqrt_rho1_ / (1 - alpha); alpha_sq_norm_ = alpha / sq_norm; } - for (int i = 0; i < static_cast(params.size()); i++) { + for (unsigned int i = 0; i < params.size(); i++) { jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i])); diff --git a/d2frontend/CMakeLists.txt b/d2frontend/CMakeLists.txt index 4f21ed29..a4541701 100644 --- a/d2frontend/CMakeLists.txt +++ b/d2frontend/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.0) project(d2frontend) set(CMAKE_CXX_STANDARD 17) diff --git a/d2frontend/include/d2frontend/utils.h b/d2frontend/include/d2frontend/utils.h index bb4fa51e..c26e771c 100644 --- a/d2frontend/include/d2frontend/utils.h +++ b/d2frontend/include/d2frontend/utils.h @@ -36,7 +36,7 @@ Eigen::VectorXf load_csv_vec_eigen(std::string csv); template inline void reduceVector(std::vector &v, std::vector status) { int j = 0; - for (int i = 0; i < int(v.size()); i++) + for (unsigned int i = 0; i < v.size(); i++) if (status[i]) v[j++] = v[i]; v.resize(j); diff --git a/d2frontend/src/CNN/superpoint.cpp b/d2frontend/src/CNN/superpoint.cpp index bd7e8c83..a9b2e977 100644 --- a/d2frontend/src/CNN/superpoint.cpp +++ b/d2frontend/src/CNN/superpoint.cpp @@ -174,7 +174,7 @@ bool SuperPoint::infer(const cv::Mat & input, std::vector & keypoin } // [n_keypoints_num,n] for (auto &descriptor : raw_descriptors) { - for (int i = 0; i < descriptor.size(); ++i) { + for (unsigned int i = 0; i < descriptor.size(); ++i) { descriptors.push_back(descriptor[i]); } } @@ -201,7 +201,7 @@ bool SuperPoint::processInput(const tensorrt_buffer::BufferManager &buffers, con void SuperPoint::findHighScoreIndex(std::vector &scores, std::vector &keypoints, int h, int w, float threshold) { std::vector new_scores; - for (int i = 0; i < scores.size(); ++i) { + for (unsigned int i = 0; i < scores.size(); ++i) { if (scores[i] > threshold) { Eigen::Vector2f location = {i % w, int(i / w)}; //u,v keypoints.emplace_back(location); @@ -217,7 +217,7 @@ void SuperPoint::removeBorders( std::vector &keypoints, std::ve int width) { std::vector keypoints_selected; std::vector scores_selected; - for (int i = 0; i < keypoints.size(); ++i) { + for (unsigned int i = 0; i < keypoints.size(); ++i) { bool flag_h = (keypoints[i][1] >= border) && (keypoints[i][1] < (height - border)); //v bool flag_w = (keypoints[i][0] >= border) && (keypoints[i][0] < (width - border)); //u if (flag_h && flag_w) { @@ -440,14 +440,14 @@ bool SuperPoint::deserializeEngine(){ // Eigen::Matrix // _desc_new = (_desc.rowwise() - pca_mean) * pca_comp_T; // // Perform row wise normalization -// for (int i = 0; i < _desc_new.rows(); i++) { +// for (unsigned int i = 0; i < _desc_new.rows(); i++) { // _desc_new.row(i) /= _desc_new.row(i).norm(); // } // local_descriptors = std::vector( // _desc_new.data(), // _desc_new.data() + _desc_new.cols() * _desc_new.rows()); // } else { -// for (int i = 0; i < _desc.rows(); i++) { +// for (unsigned int i = 0; i < _desc.rows(); i++) { // _desc.row(i) /= _desc.row(i).norm(); // } // local_descriptors = std::vector( diff --git a/d2frontend/src/CNN/superpoint_common.cpp b/d2frontend/src/CNN/superpoint_common.cpp index 592c52c5..43a99c7a 100644 --- a/d2frontend/src/CNN/superpoint_common.cpp +++ b/d2frontend/src/CNN/superpoint_common.cpp @@ -18,7 +18,7 @@ void getKeyPoints(const cv::Mat& prob, float threshold, int nms_dist, std::vector kps; cv::findNonZero(mask, kps); std::vector keypoints_no_nms; - for (int i = 0; i < kps.size(); i++) { + for (unsigned int i = 0; i < kps.size(); i++) { keypoints_no_nms.push_back(cv::Point2f(kps[i].x, kps[i].y)); } @@ -76,14 +76,14 @@ void computeDescriptors(const torch::Tensor& mProb, const torch::Tensor& mDesc, Eigen::Matrix _desc_new = (_desc.rowwise() - pca_mean) * pca_comp_T; // Perform row wise normalization - for (int i = 0; i < _desc_new.rows(); i++) { + for (unsigned int i = 0; i < _desc_new.rows(); i++) { _desc_new.row(i) /= _desc_new.row(i).norm(); } local_descriptors = std::vector( _desc_new.data(), _desc_new.data() + _desc_new.cols() * _desc_new.rows()); } else { - for (int i = 0; i < _desc.rows(); i++) { + for (unsigned int i = 0; i < _desc.rows(); i++) { _desc.row(i) /= _desc.row(i).norm(); } local_descriptors = std::vector( @@ -126,7 +126,7 @@ void NMS2(std::vector det, cv::Mat conf, confidence.at(vv, uu) = conf.at(i, 0); } - for (int i = 0; i < pts_raw.size(); i++) { + for (unsigned int i = 0; i < pts_raw.size(); i++) { int uu = (int)pts_raw[i].x; int vv = (int)pts_raw[i].y; diff --git a/d2frontend/src/d2featuretracker.cpp b/d2frontend/src/d2featuretracker.cpp index 39949274..ce675f07 100644 --- a/d2frontend/src/d2featuretracker.cpp +++ b/d2frontend/src/d2featuretracker.cpp @@ -501,7 +501,7 @@ TrackReport D2FeatureTracker::trackLK(VisualImageDesc &frame) { TrackLRType::WHOLE_IMG_MATCH); pyr_has_built = true; cur_lk_info.lk_pts_3d_norm.resize(cur_lk_info.lk_pts.size()); - for (int i = 0; i < cur_lk_info.lk_pts.size(); i++) { + for (unsigned int i = 0; i < cur_lk_info.lk_pts.size(); i++) { auto ret = createLKLandmark(frame, cur_lk_info.lk_pts[i], cur_lk_info.lk_ids[i], cur_lk_info.lk_types[i]); @@ -723,7 +723,7 @@ TrackReport D2FeatureTracker::trackLK(const VisualImageDesc &left_frame, keyframe_lk_infos.at(left_frame.frame_id).at(left_frame.camera_index); // Add the SP points to the LK points if use_lk_for_left_right_track is true if (use_lk_for_left_right_track && !_config.sp_track_use_lk) { - for (int i = 0; i < left_frame.landmarkNum(); i++) { + for (unsigned int i = 0; i < left_frame.landmarkNum(); i++) { if (left_frame.landmarks[i].landmark_id >= 0 && left_frame.landmarks[i].type == LandmarkType::SuperPointLandmark) { left_lk_info.lk_pts.emplace_back(left_frame.landmarks[i].pt2d); @@ -745,7 +745,7 @@ TrackReport D2FeatureTracker::trackLK(const VisualImageDesc &left_frame, if (!left_lk_info.lk_ids.empty()) { auto cur_lk_info = opticalflowTrackPyr(right_frame.raw_image, left_lk_info, type); - for (int i = 0; i < cur_lk_info.lk_pts.size(); i++) { + for (unsigned int i = 0; i < cur_lk_info.lk_pts.size(); i++) { auto ret = createLKLandmark(right_frame, cur_lk_info.lk_pts[i], cur_lk_info.lk_ids[i], cur_lk_info.lk_types[i]); @@ -952,7 +952,7 @@ cv::Mat D2FeatureTracker::drawToImage(const VisualImageDesc &frame, // Draw predictions here auto &predictions = landmark_predictions_viz.at(frame.camera_id); auto &prev = landmark_predictions_matched_viz.at(frame.camera_id); - for (int i = 0; i < predictions.size(); i++) { + for (unsigned int i = 0; i < predictions.size(); i++) { cv::circle(img, predictions[i], 3, cv::Scalar(0, 255, 0), 2); cv::line(img, prev[i], predictions[i], cv::Scalar(0, 0, 255), 1, 8, 0); // if (cv::norm(prev[i] - predictions[i]) > 20) { @@ -1078,7 +1078,7 @@ std::pair, std::vector> getFeatureHalfImg( float move_cols = params->width_undistort * 90.0 / params->undistort_fov; // slightly lower than 0.5 cols when fov=200 - for (int i = 0; i < pts.size(); i++) { + for (unsigned int i = 0; i < pts.size(); i++) { if (require_left && pts[i].x < params->width_undistort - move_cols || !require_left && pts[i].x >= move_cols) { tmp_to_idx[c] = i; @@ -1183,7 +1183,7 @@ bool D2FeatureTracker::matchLocalFeatures( params->width_undistort * 90.0 / params ->undistort_fov; // slightly lower than 0.5 cols when fov=200 - for (int i = 0; i < features_a.second.size(); i++) { + for (unsigned int i = 0; i < features_a.second.size(); i++) { features_a.second[i].x += param.type == LEFT_RIGHT_IMG_MATCH ? move_cols : -move_cols; } @@ -1261,10 +1261,10 @@ bool D2FeatureTracker::matchLocalFeatures( char name[100]; std::vector kps_a, kps_b; // Kps from points - for (int i = 0; i < pts_a.size(); i++) { + for (unsigned int i = 0; i < pts_a.size(); i++) { kps_a.push_back(cv::KeyPoint(pts_a[i].x, pts_a[i].y, 1)); } - for (int i = 0; i < pts_b.size(); i++) { + for (unsigned int i = 0; i < pts_b.size(); i++) { kps_b.push_back(cv::KeyPoint(pts_b[i].x, pts_b[i].y, 1)); } cv::Mat show; @@ -1283,7 +1283,7 @@ bool D2FeatureTracker::matchLocalFeatures( cv::Mat show_check; cv::hconcat(img_desc_a.raw_image, image_b, show_check); cv::cvtColor(show_check, show_check, cv::COLOR_GRAY2BGR); - for (int i = 0; i < matched_pts_a.size(); i++) { + for (unsigned int i = 0; i < matched_pts_a.size(); i++) { // random color cv::Scalar color(rand() & 255, rand() & 255, rand() & 255); cv::line(show_check, matched_pts_a[i], @@ -1320,7 +1320,7 @@ D2FeatureTracker::predictLandmarksWithExtrinsic( const Swarm::Pose &cam_pose_b) const { std::map pts_a_pred_on_b; auto cam = cams.at(camera_index); - for (int i = 0; i < pts_3d_norm.size(); i++) { + for (unsigned int i = 0; i < pts_3d_norm.size(); i++) { Vector3d landmark_pos_cam = pts_3d_norm[i] * _config.landmark_distance_assumption; Vector3d pt3d = cam_pose_a * landmark_pos_cam; @@ -1338,7 +1338,7 @@ std::vector D2FeatureTracker::predictLandmarks( std::vector pts_a_pred_on_b; assert(img_desc_a.drone_id == params->self_id); auto cam = cams.at(img_desc_a.camera_index); - for (int i = 0; i < img_desc_a.spLandmarkNum(); i++) { + for (unsigned int i = 0; i < img_desc_a.spLandmarkNum(); i++) { auto landmark_id = img_desc_a.landmarks[i].landmark_id; // Query 3d landmark position bool find_position = false; diff --git a/d2frontend/src/loop_cam.cpp b/d2frontend/src/loop_cam.cpp index e4010335..a1aeafaf 100644 --- a/d2frontend/src/loop_cam.cpp +++ b/d2frontend/src/loop_cam.cpp @@ -117,7 +117,7 @@ double triangulatePoint(Eigen::Quaterniond q0, Eigen::Vector3d t0, template void reduceVector(std::vector &v, std::vector status) { int j = 0; - for (int i = 0; i < int(v.size()); i++) + for (unsigned int i = 0; i < v.size(); i++) if (status[i]) v[j++] = v[i]; v.resize(j); } diff --git a/d2frontend/src/loop_detector.cpp b/d2frontend/src/loop_detector.cpp index eb4b3bbd..7b3ea985 100644 --- a/d2frontend/src/loop_detector.cpp +++ b/d2frontend/src/loop_detector.cpp @@ -747,7 +747,7 @@ void LoopDetector::drawMatched( } std::set inlier_set(inliers.begin(), inliers.end()); - for (int i = 0; i < index2dirindex_a.size(); i++) { + for (unsigned int i = 0; i < index2dirindex_a.size(); i++) { int old_pt_id = index2dirindex_b[i].second; int old_dir_id = index2dirindex_b[i].first; diff --git a/d2frontend/src/loop_utils.cpp b/d2frontend/src/loop_utils.cpp index b3f57e7f..a258c367 100644 --- a/d2frontend/src/loop_utils.cpp +++ b/d2frontend/src/loop_utils.cpp @@ -362,7 +362,7 @@ std::vector opticalflowTrack(const cv::Mat &cur_img, status[i] = 0; } - for (int i = 0; i < int(cur_pts.size()); i++) { + for (unsigned int i = 0; i < cur_pts.size(); i++) { if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { status[i] = 0; } @@ -468,7 +468,7 @@ LKImageInfoGPU opticalflowTrackPyr(const cv::Mat &cur_img, status[i] = 0; } - for (int i = 0; i < int(cur_pts.size()); i++) { + for (unsigned int i = 0; i < cur_pts.size(); i++) { if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { status[i] = 0; } @@ -580,7 +580,7 @@ std::vector detectFastByRegion(cv::InputArray _img, return a.response > b.response; }); // Return the top features - for (int i = 0; i < total_kpts.size(); i++) { + for (unsigned int i = 0; i < total_kpts.size(); i++) { ret.push_back(total_kpts[i].pt); if (ret.size() >= features) { break; @@ -632,7 +632,7 @@ int computeRelativePosePnP(const std::vector lm_positions_a, int iteratives = 100; Point3fVector pts3d; Point2fVector pts2d; - for (int i = 0; i < lm_positions_a.size(); i++) { + for (unsigned int i = 0; i < lm_positions_a.size(); i++) { auto z = lm_3d_norm_b[i].z(); if (z > 1e-1) { pts3d.push_back(cv::Point3f(lm_positions_a[i].x(), lm_positions_a[i].y(), @@ -676,12 +676,12 @@ Swarm::Pose computePosePnPnonCentral( opengv::points_t points; opengv::rotations_t camRotations; opengv::translations_t camOffsets; - for (int i = 0; i < lm_positions_a.size(); i++) { + for (unsigned int i = 0; i < lm_positions_a.size(); i++) { bearings.push_back(lm_3d_norm_b[i]); camCorrespondences.push_back(camera_indices[i]); points.push_back(lm_positions_a[i]); } - for (int i = 0; i < cam_extrinsics.size(); i++) { + for (unsigned int i = 0; i < cam_extrinsics.size(); i++) { camRotations.push_back(cam_extrinsics[i].R()); camOffsets.push_back(cam_extrinsics[i].pos()); } @@ -714,7 +714,7 @@ Swarm::Pose computePosePnPnonCentral( bearings.clear(); camCorrespondences.clear(); points.clear(); - for (int i = 0; i < lm_positions_a.size(); i++) { + for (unsigned int i = 0; i < lm_positions_a.size(); i++) { if (inlier_set.find(i) == inlier_set.end()) { continue; } diff --git a/d2pgo/src/d2pgo.cpp b/d2pgo/src/d2pgo.cpp index 519bd619..41e1a7e3 100644 --- a/d2pgo/src/d2pgo.cpp +++ b/d2pgo/src/d2pgo.cpp @@ -470,7 +470,7 @@ void D2PGO::setupLoopFactors(SolverWrapper* solver, void D2PGO::setupEgoMotionFactors(SolverWrapper* solver, int drone_id) { auto frames = state.getFrames(drone_id); auto traj = state.getEgomotionTraj(drone_id); - for (int i = 0; i < frames.size() - 1; i++) { + for (unsigned int i = 0; i < frames.size() - 1; i++) { auto frame_a = frames[i]; auto frame_b = frames[i + 1]; Swarm::Pose rel_pose; diff --git a/d2pgo/src/rot_init/rotation_initialization_base.hpp b/d2pgo/src/rot_init/rotation_initialization_base.hpp index e982252f..bb771b36 100644 --- a/d2pgo/src/rot_init/rotation_initialization_base.hpp +++ b/d2pgo/src/rot_init/rotation_initialization_base.hpp @@ -11,7 +11,7 @@ using D2Common::Utility::TicToc; template inline void fillInTripet(int i0, int j0, const MatrixBase & M, std::vector> & triplets) { - for (int i = 0; i < M.rows(); i ++) { //Row, col of relative rotation R + for (unsigned int i = 0; i < M.rows(); i ++) { //Row, col of relative rotation R for (int j = 0; j < M.cols(); j ++) { triplets.emplace_back(Eigen::Triplet(i0 + i, j0 + j, M(i, j))); } diff --git a/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp b/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp index 4d5caaeb..4cbc292a 100644 --- a/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp @@ -60,7 +60,7 @@ void maxCliqueHelper(CGraphIO& gio, vector* U, int sizeOfClique, // Pruning 5 if (getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq) { // Loop over U. - for (int i = 0; i < U->size(); i++) { + for (unsigned int i = 0; i < U->size(); i++) { if ((*ptrEdge)[j] == (*U)[i]) U_new.push_back((*ptrEdge)[j]); } } else @@ -146,7 +146,7 @@ int maxClique(CGraphIO& gio, int l_bound, vector& max_clique_data) { void print_max_clique(vector& max_clique_data) { // cout << "Maximum clique: "; - for (int i = 0; i < max_clique_data.size(); i++) + for (unsigned int i = 0; i < max_clique_data.size(); i++) cout << max_clique_data[i] + 1 << " "; cout << endl; } diff --git a/d2vins/src/estimator/d2estimator.cpp b/d2vins/src/estimator/d2estimator.cpp index 1d6b08a5..13a5df87 100644 --- a/d2vins/src/estimator/d2estimator.cpp +++ b/d2vins/src/estimator/d2estimator.cpp @@ -284,7 +284,7 @@ VINSFrame *D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) { } void D2Estimator::addSldWinToFrame(VisualImageDescArray &frame) { - for (int i = 0; i < state.size(); i++) { + for (unsigned int i = 0; i < state.size(); i++) { frame.sld_win_status.push_back(state.getFrame(i).frame_id); } } diff --git a/d2vins/src/estimator/d2vinsstate.cpp b/d2vins/src/estimator/d2vinsstate.cpp index 17b01b3d..de7b6cc3 100644 --- a/d2vins/src/estimator/d2vinsstate.cpp +++ b/d2vins/src/estimator/d2vinsstate.cpp @@ -70,7 +70,7 @@ std::vector D2EstimatorState::removeFrameById( } void D2EstimatorState::init(std::vector _extrinsic, double _td) { - for (int i = 0; i < _extrinsic.size(); i++) { + for (unsigned int i = 0; i < _extrinsic.size(); i++) { auto pose = _extrinsic[i]; auto cam_id = addCamera(pose, i, self_id); local_camera_ids.push_back(cam_id); @@ -658,7 +658,7 @@ void D2EstimatorState::printSldWin( const Guard lock(state_lock); for (auto it : sld_wins) { printf("=========SLDWIN@drone%d=========\n", it.first); - for (int i = 0; i < it.second.size(); i++) { + for (unsigned int i = 0; i < it.second.size(); i++) { int num_mea = 0; if (keyframe_measurments.find(it.second[i]->frame_id) != keyframe_measurments.end()) { @@ -680,7 +680,7 @@ const std::vector &D2EstimatorState::getSldWin( void D2EstimatorState::updateEgoMotion() { const Guard lock(state_lock); auto &sld_win = sld_wins[self_id]; - for (int i = 0; i < sld_win.size() - 1; i++) { + for (int i = 0; i < static_cast(sld_win.size()) - 1; i++) { auto frame_ptr = sld_win[i]; auto frame_id = sld_win[i]->frame_id; if (ego_motions.find(frame_id) == ego_motions.end()) { @@ -778,7 +778,7 @@ bool D2EstimatorState::solveGyroscopeBias( Vector3d delta_bg; A.setZero(); b.setZero(); - for (int i = 0; i < sld_win.size() - 1; i++) { + for (int i = 0; i < static_cast(sld_win.size()) - 1; i++) { auto frame_i = sld_win[i]; auto frame_j = sld_win[i + 1]; MatrixXd tmp_A(3, 3); @@ -822,7 +822,7 @@ bool D2EstimatorState::LinearAlignment( b.setZero(); int i = 0; - for (int i = 0; i < sld_win.size() - 1; i++) { + for (int i = 0; i < static_cast(sld_win.size()) - 1; i++) { auto frame_i = sld_win[i]; auto frame_j = sld_win[i + 1]; Swarm::Pose pose_i = sfm_poses.at(frame_i->frame_id); @@ -897,7 +897,7 @@ bool D2EstimatorState::LinearAlignment( g = q0 * g; SPDLOG_INFO("G final {:.4f} {:.4f} {:.4f}", g.x(), g.y(), g.z()); - for (int i = 0; i < sld_win.size(); i++) { + for (unsigned int i = 0; i < sld_win.size(); i++) { auto frame = sld_win[i]; Swarm::Pose imu_pose = sfm_poses.at(frame->frame_id); if (i == 0) { @@ -949,7 +949,7 @@ void D2EstimatorState::RefineGravity( MatrixXd lxly(3, 2); lxly = TangentBasis(g0); int i = 0; - for (int i = 0; i < sld_win.size() - 1; i++) { + for (int i = 0; i < static_cast(sld_win.size()) - 1; i++) { auto frame_i = sld_win[i]; auto frame_j = sld_win[i + 1]; Swarm::Pose pose_i = sfm_poses.at(frame_i->frame_id); diff --git a/d2vins/src/estimator/solver/ConsensusSync.cpp b/d2vins/src/estimator/solver/ConsensusSync.cpp index e75e402e..e11feeea 100644 --- a/d2vins/src/estimator/solver/ConsensusSync.cpp +++ b/d2vins/src/estimator/solver/ConsensusSync.cpp @@ -9,11 +9,11 @@ DistributedVinsData::DistributedVinsData(const DistributedVinsData_t& msg) solver_token(msg.solver_token), iteration_count(msg.iteration_count), reference_frame_id(msg.reference_frame_id) { - for (int i = 0; i < msg.frame_ids.size(); i++) { + for (unsigned int i = 0; i < msg.frame_ids.size(); i++) { frame_ids.emplace_back(msg.frame_ids[i]); frame_poses.emplace_back(Swarm::Pose(msg.frame_poses[i])); } - for (int i = 0; i < msg.extrinsic.size(); i++) { + for (unsigned int i = 0; i < msg.extrinsic.size(); i++) { extrinsic.emplace_back(Swarm::Pose(msg.extrinsic[i])); cam_ids.emplace_back(msg.cam_ids[i]); } @@ -23,11 +23,11 @@ DistributedVinsData_t DistributedVinsData::toLCM() const { DistributedVinsData_t msg; msg.timestamp = toLCMTime(ros::Time(stamp)); msg.drone_id = drone_id; - for (int i = 0; i < frame_ids.size(); i++) { + for (unsigned int i = 0; i < frame_ids.size(); i++) { msg.frame_ids.emplace_back(frame_ids[i]); msg.frame_poses.emplace_back(frame_poses[i].toLCM()); } - for (int i = 0; i < extrinsic.size(); i++) { + for (unsigned int i = 0; i < extrinsic.size(); i++) { msg.extrinsic.emplace_back(extrinsic[i].toLCM()); msg.cam_ids.emplace_back(cam_ids[i]); } diff --git a/d2vins/src/estimator/solver/VINSConsenusSolver.cpp b/d2vins/src/estimator/solver/VINSConsenusSolver.cpp index 717cfff9..af18dd67 100644 --- a/d2vins/src/estimator/solver/VINSConsenusSolver.cpp +++ b/d2vins/src/estimator/solver/VINSConsenusSolver.cpp @@ -60,7 +60,7 @@ void D2VINSConsensusSolver::receiveAll() { void D2VINSConsensusSolver::updateWithDistributedVinsData( const DistributedVinsData& dist_data) { auto _state = static_cast(state); - for (int i = 0; i < dist_data.frame_ids.size(); i++) { + for (unsigned int i = 0; i < dist_data.frame_ids.size(); i++) { auto frame_id = dist_data.frame_ids[i]; if (_state->hasFrame(frame_id)) { auto pointer = _state->getPoseState(frame_id); @@ -71,7 +71,7 @@ void D2VINSConsensusSolver::updateWithDistributedVinsData( } } - for (int i = 0; i < dist_data.cam_ids.size(); i++) { + for (unsigned int i = 0; i < dist_data.cam_ids.size(); i++) { auto cam_id = dist_data.cam_ids[i]; if (_state->hasCamera(cam_id)) { auto pointer = _state->getExtrinsicState(cam_id); diff --git a/d2vins/src/network/d2vins_net.cpp b/d2vins/src/network/d2vins_net.cpp index f3b29187..9eb026b6 100644 --- a/d2vins/src/network/d2vins_net.cpp +++ b/d2vins/src/network/d2vins_net.cpp @@ -19,7 +19,7 @@ void D2VINSNet::pubSlidingWindow() { sld_win.timestamp = toLCMTime(ros::Time(state.lastFrame().stamp)); sld_win.sld_win_len = state.size(); sld_win.drone_id = params->self_id; - for (int i = 0; i < state.size(); i++) { + for (unsigned int i = 0; i < state.size(); i++) { auto& frame = state.getFrame(i); sld_win.frame_ids.push_back(frame.frame_id); } diff --git a/d2vins/src/utils/solve_5pts.cpp b/d2vins/src/utils/solve_5pts.cpp index 52a40493..5994e798 100644 --- a/d2vins/src/utils/solve_5pts.cpp +++ b/d2vins/src/utils/solve_5pts.cpp @@ -9,7 +9,7 @@ bool MotionEstimator::solveRelativeRT( Matrix3d &Rotation, Vector3d &Translation) { if (corres.size() >= 15) { std::vector ll, rr; - for (int i = 0; i < int(corres.size()); i++) { + for (unsigned int i = 0; i < corres.size(); i++) { if (fabs(corres[i].first(2)) > 0.001 && fabs(corres[i].second(2)) > 0.001) { cv::Point2f first = diff --git a/d2vins/src/visualization/visualization.cpp b/d2vins/src/visualization/visualization.cpp index 4fe18c02..0ffe0962 100644 --- a/d2vins/src/visualization/visualization.cpp +++ b/d2vins/src/visualization/visualization.cpp @@ -48,7 +48,7 @@ void D2Visualization::init(ros::NodeHandle& nh, D2Estimator* estimator) { nh.advertise("camera_visual", 1000); frame_pub_local = nh.advertise("frame_local", 1000); frame_pub_remote = nh.advertise("frame_remote", 1000); - for (int i = 0; i < estimator->getState().localCameraExtrinsics().size(); + for (unsigned int i = 0; i < estimator->getState().localCameraExtrinsics().size(); i++) { char topic_name[64] = {0}; sprintf(topic_name, "camera_pose_%d", i); @@ -100,7 +100,7 @@ void D2Visualization::pubOdometry(int drone_id, const Swarm::Odometry& odom) { tf::StampedTransform(transform, odom_ros.header.stamp, "world", "imu")); auto& state = _estimator->getState(); auto exts = state.localCameraExtrinsics(); - for (int i = 0; i < exts.size(); i++) { + for (unsigned int i = 0; i < exts.size(); i++) { auto camera_pose = exts[i]; auto pose = odom.pose() * camera_pose; geometry_msgs::PoseStamped camera_pose_ros; @@ -179,7 +179,7 @@ void D2Visualization::postSolve() { CameraPoseVisualization sld_win_visual; for (auto drone_id : state.availableDrones()) { - for (int i = 0; i < state.size(drone_id); i++) { + for (unsigned int i = 0; i < state.size(drone_id); i++) { auto& frame = state.getFrame(drone_id, i); CamIdType camera_id = *state.getAvailableCameraIds().begin(); auto frame_pose = frame.odom.pose(); @@ -204,7 +204,7 @@ sensor_msgs::PointCloud toPointCloud( pcl.channels.resize(3); pcl.channels[0].name = "rgb"; pcl.channels[0].values.resize(landmarks.size()); - for (int i = 0; i < landmarks.size(); i++) { + for (unsigned int i = 0; i < landmarks.size(); i++) { pcl.points[i].x = landmarks[i].position.x(); pcl.points[i].y = landmarks[i].position.y(); pcl.points[i].z = landmarks[i].position.z();