From 9624f6b391e9c2efa69f28241587144e00565f17 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Wed, 24 Apr 2019 13:16:06 +0200 Subject: [PATCH] add ./scripts/clang-format-all.sh and fix formatting --- .clang-format | 7 +- include/basalt/calibration/vignette.h | 2 +- include/basalt/io/dataset_io.h | 5 +- .../basalt/optimization/spline_linearize.h | 35 ++++---- include/basalt/utils/common_types.h | 4 +- include/basalt/utils/keypoints.h | 2 +- include/basalt/utils/nfr.h | 2 +- include/basalt/utils/sim_utils.h | 6 +- include/basalt/utils/tracks.h | 5 +- scripts/clang-format-all.sh | 43 ++++++++++ src/calibrate.cpp | 1 - src/calibration/vignette.cpp | 1 - src/io/dataset_io.cpp | 1 - src/mapper.cpp | 2 +- src/mapper_sim.cpp | 3 +- src/mapper_sim_naive.cpp | 5 +- src/opt_flow.cpp | 5 +- src/optical_flow/optical_flow.cpp | 1 - src/utils/keypoints.cpp | 2 +- src/vi_estimator/ba_base.cpp | 5 +- src/vio_sim.cpp | 11 ++- test/src/test_image.cpp | 17 ++-- test/src/test_nfr.cpp | 82 +++++++++---------- test/src/test_vio.cpp | 44 +++++----- 24 files changed, 161 insertions(+), 130 deletions(-) create mode 100755 scripts/clang-format-all.sh diff --git a/.clang-format b/.clang-format index dc0134c..1d57c03 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,7 @@ --- -Language: Cpp -BasedOnStyle: Google -IndentWidth: 2 +Language: Cpp +BasedOnStyle: Google +IndentWidth: 2 +IncludeBlocks: Preserve ... diff --git a/include/basalt/calibration/vignette.h b/include/basalt/calibration/vignette.h index 15b7fb2..70bf97f 100644 --- a/include/basalt/calibration/vignette.h +++ b/include/basalt/calibration/vignette.h @@ -84,4 +84,4 @@ class VignetteEstimator { std::vector irradiance; std::vector> vign_param; }; -} +} // namespace basalt diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 6909019..9473a55 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -179,8 +179,9 @@ template ::type - load(Archive &ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, - _MaxCols> &matrix) { + load(Archive &ar, + Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> + &matrix) { std::int32_t rows; std::int32_t cols; ar(rows); diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h index cf0eb7d..d396aa2 100644 --- a/include/basalt/optimization/spline_linearize.h +++ b/include/basalt/optimization/spline_linearize.h @@ -135,9 +135,9 @@ struct LinearizeSplineOpt : public LinearizeBase { // std::cout << "time " << time << std::endl; // std::cout << "sline.minTime() " << spline.minTime() << std::endl; - BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), - "time " << time_ns << " spline.minTimeNs() " - << spline->minTimeNs()); + BASALT_ASSERT_STREAM( + time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs()); // Residual from current value of spline Vector3 residual_pos = @@ -207,12 +207,12 @@ struct LinearizeSplineOpt : public LinearizeBase { // std::cout << "time " << t << std::endl; // std::cout << "sline.minTime() " << spline.minTime() << std::endl; - BASALT_ASSERT_STREAM(t >= spline->minTimeNs(), - "t " << t << " spline.minTime() " - << spline->minTimeNs()); - BASALT_ASSERT_STREAM(t <= spline->maxTimeNs(), - "t " << t << " spline.maxTime() " - << spline->maxTimeNs()); + BASALT_ASSERT_STREAM( + t >= spline->minTimeNs(), + "t " << t << " spline.minTime() " << spline->minTimeNs()); + BASALT_ASSERT_STREAM( + t <= spline->maxTimeNs(), + "t " << t << " spline.maxTime() " << spline->maxTimeNs()); Vector3 residual = spline->accelResidual( t, pm.data, this->common_data.calibration->calib_accel_bias, @@ -255,8 +255,9 @@ struct LinearizeSplineOpt : public LinearizeBase { BASALT_ASSERT(start_j < opt_size); accum.template addH( - start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() * - J.d_val_d_knot[j]); + start_i, start_j, + accel_var_inv * J.d_val_d_knot[i].transpose() * + J.d_val_d_knot[j]); } accum.template addH( start_bias, start_i, @@ -497,9 +498,9 @@ struct LinearizeSplineOpt : public LinearizeBase { if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) continue; - BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), - "time " << time_ns << " spline.minTimeNs() " - << spline->minTimeNs()); + BASALT_ASSERT_STREAM( + time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs()); const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w; const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark; @@ -563,9 +564,9 @@ struct LinearizeSplineOpt : public LinearizeBase { d_res_d_T_w_i.transpose() * residual); accum.template addH( - start_T_moc_w, start_i, mocap_var_inv * - d_res_d_T_moc_w.transpose() * - d_res_d_T_w_i * J_pose.d_val_d_knot[i]); + start_T_moc_w, start_i, + mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i * + J_pose.d_val_d_knot[i]); accum.template addH( start_T_i_mark, start_i, diff --git a/include/basalt/utils/common_types.h b/include/basalt/utils/common_types.h index c9a543e..00526bf 100644 --- a/include/basalt/utils/common_types.h +++ b/include/basalt/utils/common_types.h @@ -238,7 +238,7 @@ using ImageProjections = std::map; /// inlier projections indexed per track using TrackProjections = std::unordered_map>; -} +} // namespace basalt namespace cereal { @@ -251,4 +251,4 @@ template void serialize(Archive& ar, basalt::MatchData& c) { ar(c.T_i_j, c.matches, c.inliers); } -} +} // namespace cereal diff --git a/include/basalt/utils/keypoints.h b/include/basalt/utils/keypoints.h index c556477..3cd3768 100644 --- a/include/basalt/utils/keypoints.h +++ b/include/basalt/utils/keypoints.h @@ -53,7 +53,7 @@ namespace basalt { typedef std::bitset<256> Descriptor; void detectKeypointsMapping(const basalt::Image& img_raw, - KeypointsData& kd, int num_features); + KeypointsData& kd, int num_features); void detectKeypoints(const basalt::Image& img_raw, KeypointsData& kd, int PATCH_SIZE = 32, diff --git a/include/basalt/utils/nfr.h b/include/basalt/utils/nfr.h index 24d9fab..8f8095f 100644 --- a/include/basalt/utils/nfr.h +++ b/include/basalt/utils/nfr.h @@ -117,4 +117,4 @@ inline Sophus::Vector2d rollPitchError( return res.head<2>(); } -} +} // namespace basalt diff --git a/include/basalt/utils/sim_utils.h b/include/basalt/utils/sim_utils.h index dc7dc42..15b5806 100644 --- a/include/basalt/utils/sim_utils.h +++ b/include/basalt/utils/sim_utils.h @@ -43,13 +43,11 @@ struct SimObservations { std::vector id; }; -} +} // namespace basalt namespace cereal { template void serialize(Archive& ar, basalt::SimObservations& c) { ar(c.pos, c.id); } -} - - +} // namespace cereal diff --git a/include/basalt/utils/tracks.h b/include/basalt/utils/tracks.h index b6a4b83..72b2106 100644 --- a/include/basalt/utils/tracks.h +++ b/include/basalt/utils/tracks.h @@ -47,11 +47,8 @@ #include #include - namespace basalt { - - /// TrackBuild class creates feature tracks from matches struct TrackBuilder { std::map map_node_to_index; @@ -222,4 +219,4 @@ bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks, return !track_ids.empty(); } -} +} // namespace basalt diff --git a/scripts/clang-format-all.sh b/scripts/clang-format-all.sh new file mode 100755 index 0000000..79b7bea --- /dev/null +++ b/scripts/clang-format-all.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash + +# Format all source files in the project. +# Optionally take folder as argument; default is full inlude and src dirs. + +set -e + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" + +FOLDER="${1:-$SCRIPT_DIR/../include $SCRIPT_DIR/../src $SCRIPT_DIR/../test/src}" + +CLANG_FORMAT_COMMANDS="clang-format-9 clang-format-8 clang-format" + +# find the first available command: +for CMD in $CLANG_FORMAT_COMMANDS; do + if hash $CMD 2>/dev/null; then + CLANG_FORMAT_CMD=$CMD + break + fi +done + +if [ -z $CLANG_FORMAT_CMD ]; then + echo "clang-format not installed..." + exit 1 +fi + +# clang format check version +MAJOR_VERSION_NEEDED=8 + +MAJOR_VERSION_DETECTED=`$CLANG_FORMAT_CMD -version | sed -n -E 's/.*version ([0-9]+).*/\1/p'` +if [ -z $MAJOR_VERSION_DETECTED ]; then + echo "Failed to parse major version (`$CLANG_FORMAT_CMD -version`)" + exit 1 +fi + +echo "clang-format version $MAJOR_VERSION_DETECTED (`$CLANG_FORMAT_CMD -version`)" + +if [ $MAJOR_VERSION_DETECTED -lt $MAJOR_VERSION_NEEDED ]; then + echo "Looks like your clang format is too old; need at least version $MAJOR_VERSION_NEEDED" + exit 1 +fi + +find $FOLDER -iname "*.?pp" -or -iname "*.h" | xargs $CLANG_FORMAT_CMD -verbose -i diff --git a/src/calibrate.cpp b/src/calibrate.cpp index 8555328..00dabca 100644 --- a/src/calibrate.cpp +++ b/src/calibrate.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp index 0a8619a..520c4e4 100644 --- a/src/calibration/vignette.cpp +++ b/src/calibration/vignette.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include namespace basalt { diff --git a/src/io/dataset_io.cpp b/src/io/dataset_io.cpp index ac1a044..f49fbaf 100644 --- a/src/io/dataset_io.cpp +++ b/src/io/dataset_io.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include #include diff --git a/src/mapper.cpp b/src/mapper.cpp index 158f4a5..fa6ae57 100644 --- a/src/mapper.cpp +++ b/src/mapper.cpp @@ -323,7 +323,7 @@ int main(int argc, char** argv) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } } else { - //optimize(); + // optimize(); detect(); match(); tracks(); diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp index 4d47b5a..9907870 100644 --- a/src/mapper_sim.cpp +++ b/src/mapper_sim.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include #include @@ -120,7 +119,7 @@ double alignButton(); void setup_points(); constexpr int UI_WIDTH = 200; -//constexpr int NUM_FRAMES = 500; +// constexpr int NUM_FRAMES = 500; basalt::Calibration calib; diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index 2f06d76..6a184c8 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include #include @@ -492,8 +491,8 @@ void load_data(const std::string& calib_path) { if (os.is_open()) { cereal::JSONInputArchive archive(os); archive(calib); - std::cout << "Loaded camera with " << calib.intrinsics.size() - << " cameras" << std::endl; + std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras" + << std::endl; } else { std::cerr << "could not load camera calibration " << calib_path diff --git a/src/opt_flow.cpp b/src/opt_flow.cpp index 3cc41b4..ab74b3e 100644 --- a/src/opt_flow.cpp +++ b/src/opt_flow.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include #include @@ -321,8 +320,8 @@ void load_data(const std::string& calib_path) { if (os.is_open()) { cereal::JSONInputArchive archive(os); archive(calib); - std::cout << "Loaded camera with " << calib.intrinsics.size() - << " cameras" << std::endl; + std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras" + << std::endl; } else { std::cerr << "could not load camera calibration " << calib_path diff --git a/src/optical_flow/optical_flow.cpp b/src/optical_flow/optical_flow.cpp index 7861fae..e68d09a 100644 --- a/src/optical_flow/optical_flow.cpp +++ b/src/optical_flow/optical_flow.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include diff --git a/src/utils/keypoints.cpp b/src/utils/keypoints.cpp index f7336f1..9d7e768 100644 --- a/src/utils/keypoints.cpp +++ b/src/utils/keypoints.cpp @@ -128,7 +128,7 @@ const static char pattern_31_y_b[256] = { -11}; void detectKeypointsMapping(const basalt::Image& img_raw, - KeypointsData& kd, int num_features) { + KeypointsData& kd, int num_features) { cv::Mat image(img_raw.h, img_raw.w, CV_8U); uint8_t* dst = image.ptr(); diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 82b480d..0d93f5b 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -33,8 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - - #include #include @@ -354,7 +352,6 @@ void BundleAdjustmentBase::linearizeHelper( } } } - }); for (const auto& rld : rld_vec) error += rld.error; @@ -522,4 +519,4 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H, abs_H.resize(0, 0); abs_b.resize(0); } -} +} // namespace basalt diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index 10ec81c..4b79f65 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include #include #include @@ -541,8 +540,8 @@ void load_data(const std::string& calib_path) { if (os.is_open()) { cereal::JSONInputArchive archive(os); archive(calib); - std::cout << "Loaded camera with " << calib.intrinsics.size() - << " cameras" << std::endl; + std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras" + << std::endl; } else { std::cerr << "could not load camera calibration " << calib_path @@ -595,9 +594,9 @@ void compute_projections() { void gen_data() { for (size_t i = 0; i < calib.intrinsics.size(); i++) { images.emplace_back(); - images.back() = pangolin::TypedImage( - calib.resolution[i][0], calib.resolution[i][1], - pangolin::PixelFormatFromString("GRAY8")); + images.back() = + pangolin::TypedImage(calib.resolution[i][0], calib.resolution[i][1], + pangolin::PixelFormatFromString("GRAY8")); images.back().Fill(200); } diff --git a/test/src/test_image.cpp b/test/src/test_image.cpp index ab47c20..e1f1375 100644 --- a/test/src/test_image.cpp +++ b/test/src/test_image.cpp @@ -27,14 +27,15 @@ TEST(Pattern, ImageInterp) { // std::cerr << "vg\n" << vg << std::endl; - test_jacobian("d_val_d_p", J, - [&](const Eigen::Vector2d &x) { - Eigen::Matrix res; - Eigen::Vector2d p1 = offset + x; - res[0] = img.interpGrad(p1)[0]; - return res; - }, - Eigen::Vector2d::Zero(), 1.0); + test_jacobian( + "d_val_d_p", J, + [&](const Eigen::Vector2d &x) { + Eigen::Matrix res; + Eigen::Vector2d p1 = offset + x; + res[0] = img.interpGrad(p1)[0]; + return res; + }, + Eigen::Vector2d::Zero(), 1.0); } TEST(Image, ImageInterpolate) { diff --git a/test/src/test_nfr.cpp b/test/src/test_nfr.cpp index ebf6fea..7f28686 100644 --- a/test/src/test_nfr.cpp +++ b/test/src/test_nfr.cpp @@ -34,29 +34,29 @@ TEST(PreIntegrationTestSuite, RelPoseTest) { { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, - [&](const Sophus::Vector6d& x) { - auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + test_jacobian( + "d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); - return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j); - - }, - x0); + return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j); + }, + x0); } { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_T_w_j", d_res_d_T_w_j, - [&](const Sophus::Vector6d& x) { - auto T_w_j_new = T_w_j; - basalt::PoseState::incPose(x, T_w_j_new); + test_jacobian( + "d_res_d_T_w_j", d_res_d_T_w_j, + [&](const Sophus::Vector6d& x) { + auto T_w_j_new = T_w_j; + basalt::PoseState::incPose(x, T_w_j_new); - return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new); - - }, - x0); + return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new); + }, + x0); } } @@ -71,15 +71,15 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) { { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, - [&](const Sophus::Vector6d& x) { - auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + test_jacobian( + "d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); - return basalt::absPositionError(T_w_i_new, pos); - - }, - x0); + return basalt::absPositionError(T_w_i_new, pos); + }, + x0); } } @@ -97,17 +97,17 @@ TEST(PreIntegrationTestSuite, YawTest) { { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, - [&](const Sophus::Vector6d& x) { - auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + test_jacobian( + "d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); - double res = basalt::yawError(T_w_i_new, yaw_dir_body); + double res = basalt::yawError(T_w_i_new, yaw_dir_body); - return Eigen::Matrix(res); - - }, - x0); + return Eigen::Matrix(res); + }, + x0); } } @@ -124,14 +124,14 @@ TEST(PreIntegrationTestSuite, RollPitchTest) { { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, - [&](const Sophus::Vector6d& x) { - auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + test_jacobian( + "d_res_d_T_w_i", d_res_d_T_w_i, + [&](const Sophus::Vector6d& x) { + auto T_w_i_new = T_w_i; + basalt::PoseState::incPose(x, T_w_i_new); - return basalt::rollPitchError(T_w_i_new, R_w_i); - - }, - x0); + return basalt::rollPitchError(T_w_i_new, R_w_i); + }, + x0); } } diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp index ab039fa..68b9a90 100644 --- a/test/src/test_vio.cpp +++ b/test/src/test_vio.cpp @@ -372,37 +372,37 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) { { Sophus::Vector6d x0; x0.setZero(); - test_jacobian("d_res_d_xi", d_res_d_xi, - [&](const Sophus::Vector6d& x) { - Eigen::Matrix4d T_t_h_new = - (Sophus::expd(x) * T_t_h_sophus).matrix(); + test_jacobian( + "d_res_d_xi", d_res_d_xi, + [&](const Sophus::Vector6d& x) { + Eigen::Matrix4d T_t_h_new = (Sophus::expd(x) * T_t_h_sophus).matrix(); - Eigen::Vector2d res; - basalt::KeypointVioEstimator::linearizePoint( - kpt_obs, kpt_pos, T_t_h_new, cam, res); + Eigen::Vector2d res; + basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos, + T_t_h_new, cam, res); - return res; - }, - x0); + return res; + }, + x0); } { Eigen::Vector3d x0; x0.setZero(); - test_jacobian("d_res_d_p", d_res_d_p, - [&](const Eigen::Vector3d& x) { - basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = - kpt_pos; + test_jacobian( + "d_res_d_p", d_res_d_p, + [&](const Eigen::Vector3d& x) { + basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos; - kpt_pos_new.dir += x.head<2>(); - kpt_pos_new.id += x[2]; + kpt_pos_new.dir += x.head<2>(); + kpt_pos_new.id += x[2]; - Eigen::Vector2d res; - basalt::KeypointVioEstimator::linearizePoint( - kpt_obs, kpt_pos_new, T_t_h, cam, res); + Eigen::Vector2d res; + basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos_new, + T_t_h, cam, res); - return res; - }, - x0); + return res; + }, + x0); } }