add ./scripts/clang-format-all.sh and fix formatting

This commit is contained in:
Nikolaus Demmel 2019-04-24 13:16:06 +02:00
parent ac80d2ea30
commit 9624f6b391
24 changed files with 161 additions and 130 deletions

View File

@ -1,6 +1,7 @@
--- ---
Language: Cpp Language: Cpp
BasedOnStyle: Google BasedOnStyle: Google
IndentWidth: 2 IndentWidth: 2
IncludeBlocks: Preserve
... ...

View File

@ -84,4 +84,4 @@ class VignetteEstimator {
std::vector<double> irradiance; std::vector<double> irradiance;
std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param; std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param;
}; };
} } // namespace basalt

View File

@ -179,8 +179,9 @@ template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
inline inline
typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic, typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic,
void>::type void>::type
load(Archive &ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, load(Archive &ar,
_MaxCols> &matrix) { Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
&matrix) {
std::int32_t rows; std::int32_t rows;
std::int32_t cols; std::int32_t cols;
ar(rows); ar(rows);

View File

@ -135,9 +135,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
// std::cout << "time " << time << std::endl; // std::cout << "time " << time << std::endl;
// std::cout << "sline.minTime() " << spline.minTime() << std::endl; // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), BASALT_ASSERT_STREAM(
"time " << time_ns << " spline.minTimeNs() " time_ns >= spline->minTimeNs(),
<< spline->minTimeNs()); "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
// Residual from current value of spline // Residual from current value of spline
Vector3 residual_pos = Vector3 residual_pos =
@ -207,12 +207,12 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
// std::cout << "time " << t << std::endl; // std::cout << "time " << t << std::endl;
// std::cout << "sline.minTime() " << spline.minTime() << std::endl; // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
BASALT_ASSERT_STREAM(t >= spline->minTimeNs(), BASALT_ASSERT_STREAM(
"t " << t << " spline.minTime() " t >= spline->minTimeNs(),
<< spline->minTimeNs()); "t " << t << " spline.minTime() " << spline->minTimeNs());
BASALT_ASSERT_STREAM(t <= spline->maxTimeNs(), BASALT_ASSERT_STREAM(
"t " << t << " spline.maxTime() " t <= spline->maxTimeNs(),
<< spline->maxTimeNs()); "t " << t << " spline.maxTime() " << spline->maxTimeNs());
Vector3 residual = spline->accelResidual( Vector3 residual = spline->accelResidual(
t, pm.data, this->common_data.calibration->calib_accel_bias, t, pm.data, this->common_data.calibration->calib_accel_bias,
@ -255,8 +255,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
BASALT_ASSERT(start_j < opt_size); BASALT_ASSERT(start_j < opt_size);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() * start_i, start_j,
J.d_val_d_knot[j]); accel_var_inv * J.d_val_d_knot[i].transpose() *
J.d_val_d_knot[j]);
} }
accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>( accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
start_bias, start_i, start_bias, start_i,
@ -497,9 +498,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
continue; continue;
BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(), BASALT_ASSERT_STREAM(
"time " << time_ns << " spline.minTimeNs() " time_ns >= spline->minTimeNs(),
<< 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_moc_w = this->common_data.mocap_calibration->T_moc_w;
const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark; const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
@ -563,9 +564,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
d_res_d_T_w_i.transpose() * residual); d_res_d_T_w_i.transpose() * residual);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
start_T_moc_w, start_i, mocap_var_inv * start_T_moc_w, start_i,
d_res_d_T_moc_w.transpose() * mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i *
d_res_d_T_w_i * J_pose.d_val_d_knot[i]); J_pose.d_val_d_knot[i]);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
start_T_i_mark, start_i, start_T_i_mark, start_i,

View File

@ -238,7 +238,7 @@ using ImageProjections = std::map<TimeCamId, ImageProjection>;
/// inlier projections indexed per track /// inlier projections indexed per track
using TrackProjections = using TrackProjections =
std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>; std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
} } // namespace basalt
namespace cereal { namespace cereal {
@ -251,4 +251,4 @@ template <class Archive>
void serialize(Archive& ar, basalt::MatchData& c) { void serialize(Archive& ar, basalt::MatchData& c) {
ar(c.T_i_j, c.matches, c.inliers); ar(c.T_i_j, c.matches, c.inliers);
} }
} } // namespace cereal

View File

@ -53,7 +53,7 @@ namespace basalt {
typedef std::bitset<256> Descriptor; typedef std::bitset<256> Descriptor;
void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw, void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, int num_features); KeypointsData& kd, int num_features);
void detectKeypoints(const basalt::Image<const uint16_t>& img_raw, void detectKeypoints(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, int PATCH_SIZE = 32, KeypointsData& kd, int PATCH_SIZE = 32,

View File

@ -117,4 +117,4 @@ inline Sophus::Vector2d rollPitchError(
return res.head<2>(); return res.head<2>();
} }
} } // namespace basalt

View File

@ -43,13 +43,11 @@ struct SimObservations {
std::vector<int> id; std::vector<int> id;
}; };
} } // namespace basalt
namespace cereal { namespace cereal {
template <class Archive> template <class Archive>
void serialize(Archive& ar, basalt::SimObservations& c) { void serialize(Archive& ar, basalt::SimObservations& c) {
ar(c.pos, c.id); ar(c.pos, c.id);
} }
} } // namespace cereal

View File

@ -47,11 +47,8 @@
#include <basalt/utils/common_types.h> #include <basalt/utils/common_types.h>
#include <basalt/utils/union_find.h> #include <basalt/utils/union_find.h>
namespace basalt { namespace basalt {
/// TrackBuild class creates feature tracks from matches /// TrackBuild class creates feature tracks from matches
struct TrackBuilder { struct TrackBuilder {
std::map<ImageFeaturePair, TrackId> map_node_to_index; std::map<ImageFeaturePair, TrackId> map_node_to_index;
@ -222,4 +219,4 @@ bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks,
return !track_ids.empty(); return !track_ids.empty();
} }
} } // namespace basalt

43
scripts/clang-format-all.sh Executable file
View File

@ -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

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/calibration/cam_calib.h> #include <basalt/calibration/cam_calib.h>
#include <tbb/tbb.h> #include <tbb/tbb.h>

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/calibration/vignette.h> #include <basalt/calibration/vignette.h>
namespace basalt { namespace basalt {

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/io/dataset_io.h> #include <basalt/io/dataset_io.h>
#include <basalt/io/dataset_io_euroc.h> #include <basalt/io/dataset_io_euroc.h>
#include <basalt/io/dataset_io_rosbag.h> #include <basalt/io/dataset_io_rosbag.h>

View File

@ -323,7 +323,7 @@ int main(int argc, char** argv) {
std::this_thread::sleep_for(std::chrono::milliseconds(50)); std::this_thread::sleep_for(std::chrono::milliseconds(50));
} }
} else { } else {
//optimize(); // optimize();
detect(); detect();
match(); match();
tracks(); tracks();

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <algorithm> #include <algorithm>
#include <chrono> #include <chrono>
#include <iostream> #include <iostream>
@ -120,7 +119,7 @@ double alignButton();
void setup_points(); void setup_points();
constexpr int UI_WIDTH = 200; constexpr int UI_WIDTH = 200;
//constexpr int NUM_FRAMES = 500; // constexpr int NUM_FRAMES = 500;
basalt::Calibration<double> calib; basalt::Calibration<double> calib;

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <algorithm> #include <algorithm>
#include <chrono> #include <chrono>
#include <iostream> #include <iostream>
@ -492,8 +491,8 @@ void load_data(const std::string& calib_path) {
if (os.is_open()) { if (os.is_open()) {
cereal::JSONInputArchive archive(os); cereal::JSONInputArchive archive(os);
archive(calib); archive(calib);
std::cout << "Loaded camera with " << calib.intrinsics.size() std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
<< " cameras" << std::endl; << std::endl;
} else { } else {
std::cerr << "could not load camera calibration " << calib_path std::cerr << "could not load camera calibration " << calib_path

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <algorithm> #include <algorithm>
#include <chrono> #include <chrono>
#include <iostream> #include <iostream>
@ -321,8 +320,8 @@ void load_data(const std::string& calib_path) {
if (os.is_open()) { if (os.is_open()) {
cereal::JSONInputArchive archive(os); cereal::JSONInputArchive archive(os);
archive(calib); archive(calib);
std::cout << "Loaded camera with " << calib.intrinsics.size() std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
<< " cameras" << std::endl; << std::endl;
} else { } else {
std::cerr << "could not load camera calibration " << calib_path std::cerr << "could not load camera calibration " << calib_path

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/optical_flow/optical_flow.h> #include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/frame_to_frame_optical_flow.h> #include <basalt/optical_flow/frame_to_frame_optical_flow.h>

View File

@ -128,7 +128,7 @@ const static char pattern_31_y_b[256] = {
-11}; -11};
void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw, void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, int num_features) { KeypointsData& kd, int num_features) {
cv::Mat image(img_raw.h, img_raw.w, CV_8U); cv::Mat image(img_raw.h, img_raw.w, CV_8U);
uint8_t* dst = image.ptr(); uint8_t* dst = image.ptr();

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/vi_estimator/ba_base.h> #include <basalt/vi_estimator/ba_base.h>
#include <tbb/parallel_for.h> #include <tbb/parallel_for.h>
@ -354,7 +352,6 @@ void BundleAdjustmentBase::linearizeHelper(
} }
} }
} }
}); });
for (const auto& rld : rld_vec) error += rld.error; 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_H.resize(0, 0);
abs_b.resize(0); abs_b.resize(0);
} }
} } // namespace basalt

View File

@ -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. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <algorithm> #include <algorithm>
#include <chrono> #include <chrono>
#include <iostream> #include <iostream>
@ -541,8 +540,8 @@ void load_data(const std::string& calib_path) {
if (os.is_open()) { if (os.is_open()) {
cereal::JSONInputArchive archive(os); cereal::JSONInputArchive archive(os);
archive(calib); archive(calib);
std::cout << "Loaded camera with " << calib.intrinsics.size() std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
<< " cameras" << std::endl; << std::endl;
} else { } else {
std::cerr << "could not load camera calibration " << calib_path std::cerr << "could not load camera calibration " << calib_path
@ -595,9 +594,9 @@ void compute_projections() {
void gen_data() { void gen_data() {
for (size_t i = 0; i < calib.intrinsics.size(); i++) { for (size_t i = 0; i < calib.intrinsics.size(); i++) {
images.emplace_back(); images.emplace_back();
images.back() = pangolin::TypedImage( images.back() =
calib.resolution[i][0], calib.resolution[i][1], pangolin::TypedImage(calib.resolution[i][0], calib.resolution[i][1],
pangolin::PixelFormatFromString("GRAY8")); pangolin::PixelFormatFromString("GRAY8"));
images.back().Fill(200); images.back().Fill(200);
} }

View File

@ -27,14 +27,15 @@ TEST(Pattern, ImageInterp) {
// std::cerr << "vg\n" << vg << std::endl; // std::cerr << "vg\n" << vg << std::endl;
test_jacobian("d_val_d_p", J, test_jacobian(
[&](const Eigen::Vector2d &x) { "d_val_d_p", J,
Eigen::Matrix<double, 1, 1> res; [&](const Eigen::Vector2d &x) {
Eigen::Vector2d p1 = offset + x; Eigen::Matrix<double, 1, 1> res;
res[0] = img.interpGrad(p1)[0]; Eigen::Vector2d p1 = offset + x;
return res; res[0] = img.interpGrad(p1)[0];
}, return res;
Eigen::Vector2d::Zero(), 1.0); },
Eigen::Vector2d::Zero(), 1.0);
} }
TEST(Image, ImageInterpolate) { TEST(Image, ImageInterpolate) {

View File

@ -34,29 +34,29 @@ TEST(PreIntegrationTestSuite, RelPoseTest) {
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_T_w_i", d_res_d_T_w_i,
auto T_w_i_new = T_w_i; [&](const Sophus::Vector6d& x) {
basalt::PoseState::incPose(x, T_w_i_new); 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); return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j);
},
}, x0);
x0);
} }
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_T_w_j", d_res_d_T_w_j, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_T_w_j", d_res_d_T_w_j,
auto T_w_j_new = T_w_j; [&](const Sophus::Vector6d& x) {
basalt::PoseState::incPose(x, T_w_j_new); 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); return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new);
},
}, x0);
x0);
} }
} }
@ -71,15 +71,15 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) {
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_T_w_i", d_res_d_T_w_i,
auto T_w_i_new = T_w_i; [&](const Sophus::Vector6d& x) {
basalt::PoseState::incPose(x, T_w_i_new); auto T_w_i_new = T_w_i;
basalt::PoseState::incPose(x, T_w_i_new);
return basalt::absPositionError(T_w_i_new, pos); return basalt::absPositionError(T_w_i_new, pos);
},
}, x0);
x0);
} }
} }
@ -97,17 +97,17 @@ TEST(PreIntegrationTestSuite, YawTest) {
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_T_w_i", d_res_d_T_w_i,
auto T_w_i_new = T_w_i; [&](const Sophus::Vector6d& x) {
basalt::PoseState::incPose(x, T_w_i_new); 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<double, 1, 1>(res); return Eigen::Matrix<double, 1, 1>(res);
},
}, x0);
x0);
} }
} }
@ -124,14 +124,14 @@ TEST(PreIntegrationTestSuite, RollPitchTest) {
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_T_w_i", d_res_d_T_w_i,
auto T_w_i_new = T_w_i; [&](const Sophus::Vector6d& x) {
basalt::PoseState::incPose(x, T_w_i_new); 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); return basalt::rollPitchError(T_w_i_new, R_w_i);
},
}, x0);
x0);
} }
} }

View File

@ -372,37 +372,37 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
{ {
Sophus::Vector6d x0; Sophus::Vector6d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_xi", d_res_d_xi, test_jacobian(
[&](const Sophus::Vector6d& x) { "d_res_d_xi", d_res_d_xi,
Eigen::Matrix4d T_t_h_new = [&](const Sophus::Vector6d& x) {
(Sophus::expd(x) * T_t_h_sophus).matrix(); Eigen::Matrix4d T_t_h_new = (Sophus::expd(x) * T_t_h_sophus).matrix();
Eigen::Vector2d res; Eigen::Vector2d res;
basalt::KeypointVioEstimator::linearizePoint( basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos,
kpt_obs, kpt_pos, T_t_h_new, cam, res); T_t_h_new, cam, res);
return res; return res;
}, },
x0); x0);
} }
{ {
Eigen::Vector3d x0; Eigen::Vector3d x0;
x0.setZero(); x0.setZero();
test_jacobian("d_res_d_p", d_res_d_p, test_jacobian(
[&](const Eigen::Vector3d& x) { "d_res_d_p", d_res_d_p,
basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = [&](const Eigen::Vector3d& x) {
kpt_pos; basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos;
kpt_pos_new.dir += x.head<2>(); kpt_pos_new.dir += x.head<2>();
kpt_pos_new.id += x[2]; kpt_pos_new.id += x[2];
Eigen::Vector2d res; Eigen::Vector2d res;
basalt::KeypointVioEstimator::linearizePoint( basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos_new,
kpt_obs, kpt_pos_new, T_t_h, cam, res); T_t_h, cam, res);
return res; return res;
}, },
x0); x0);
} }
} }