diff --git a/include/basalt/calibration/aprilgrid.h b/include/basalt/calibration/aprilgrid.h index 3c4a885..dc618e4 100644 --- a/include/basalt/calibration/aprilgrid.h +++ b/include/basalt/calibration/aprilgrid.h @@ -41,8 +41,8 @@ namespace basalt { struct AprilGrid { AprilGrid(const std::string &config_path); - Eigen::vector aprilgrid_corner_pos_3d; - Eigen::vector aprilgrid_vignette_pos_3d; + Eigen::aligned_vector aprilgrid_corner_pos_3d; + Eigen::aligned_vector aprilgrid_vignette_pos_3d; private: int tagCols; // number of apriltags diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h index c88689a..a71ebc9 100644 --- a/include/basalt/calibration/calibration_helper.h +++ b/include/basalt/calibration/calibration_helper.h @@ -44,7 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { struct CalibCornerData { - Eigen::vector corners; + Eigen::aligned_vector corners; std::vector corner_ids; std::vector radii; //!< threshold used for maximum displacement //! during sub-pix refinement; Search region is @@ -53,7 +53,7 @@ struct CalibCornerData { }; struct ProjectedCornerData { - Eigen::vector corners_proj; + Eigen::aligned_vector corners_proj; std::vector corners_proj_success; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -63,7 +63,7 @@ struct CalibInitPoseData { Sophus::SE3d T_a_c; size_t num_inliers; - Eigen::vector reprojected_corners; + Eigen::aligned_vector reprojected_corners; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -78,16 +78,16 @@ class CalibHelper { static void initCamPoses( const Calibration::Ptr& calib, - const Eigen::vector& aprilgrid_corner_pos_3d, + const Eigen::aligned_vector& aprilgrid_corner_pos_3d, tbb::concurrent_unordered_map& calib_corners, tbb::concurrent_unordered_map& calib_init_poses); static bool initializeIntrinsics( - const Eigen::vector& corners, + const Eigen::aligned_vector& corners, const std::vector& corner_ids, - const Eigen::vector& aprilgrid_corner_pos_3d, int cols, - int rows, Eigen::Vector4d& init_intr); + const Eigen::aligned_vector& aprilgrid_corner_pos_3d, + int cols, int rows, Eigen::Vector4d& init_intr); private: inline static double square(double x) { return x * x; } @@ -98,14 +98,14 @@ class CalibHelper { static void computeInitialPose( const Calibration::Ptr& calib, size_t cam_id, - const Eigen::vector& aprilgrid_corner_pos_3d, + const Eigen::aligned_vector& aprilgrid_corner_pos_3d, const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp); static size_t computeReprojectionError( const UnifiedCamera& cam_calib, - const Eigen::vector& corners, + const Eigen::aligned_vector& corners, const std::vector& corner_ids, - const Eigen::vector& aprilgrid_corner_pos_3d, + const Eigen::aligned_vector& aprilgrid_corner_pos_3d, const Sophus::SE3d& T_target_camera, double& error); }; diff --git a/include/basalt/calibration/vignette.h b/include/basalt/calibration/vignette.h index 584cc56..8f1ed20 100644 --- a/include/basalt/calibration/vignette.h +++ b/include/basalt/calibration/vignette.h @@ -47,12 +47,13 @@ class VignetteEstimator { static const int64_t knot_spacing = 1e10; static const int border_size = 2; - VignetteEstimator(const VioDatasetPtr &vio_dataset, - const Eigen::vector &optical_centers, - const Eigen::vector &resolutions, - const std::map> - &reprojected_vignette, - const AprilGrid &april_grid); + VignetteEstimator( + const VioDatasetPtr &vio_dataset, + const Eigen::aligned_vector &optical_centers, + const Eigen::aligned_vector &resolutions, + const std::map> + &reprojected_vignette, + const AprilGrid &april_grid); void compute_error(std::map> *reprojected_vignette_error = nullptr); @@ -75,9 +76,10 @@ class VignetteEstimator { private: const VioDatasetPtr vio_dataset; - Eigen::vector optical_centers; - Eigen::vector resolutions; - std::map> reprojected_vignette; + Eigen::aligned_vector optical_centers; + Eigen::aligned_vector resolutions; + std::map> + reprojected_vignette; const AprilGrid &april_grid; size_t vign_size; diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index 49d440b..de00043 100644 --- a/include/basalt/device/rs_t265.h +++ b/include/basalt/device/rs_t265.h @@ -105,7 +105,7 @@ class RsT265Device { int frame_counter = 0; - Eigen::deque gyro_data_queue; + Eigen::aligned_deque gyro_data_queue; std::shared_ptr prev_accel_data; std::shared_ptr> calib; diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 43b7fbb..7aa8458 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -79,7 +79,7 @@ struct ImageData { }; struct Observations { - Eigen::vector pos; + Eigen::aligned_vector pos; std::vector id; }; @@ -111,7 +111,7 @@ struct AprilgridCornersData { int64_t timestamp_ns; int cam_id; - Eigen::vector corner_pos; + Eigen::aligned_vector corner_pos; std::vector corner_id; }; @@ -123,10 +123,11 @@ class VioDataset { virtual std::vector &get_image_timestamps() = 0; - virtual const Eigen::vector &get_accel_data() const = 0; - virtual const Eigen::vector &get_gyro_data() const = 0; + virtual const Eigen::aligned_vector &get_accel_data() const = 0; + virtual const Eigen::aligned_vector &get_gyro_data() const = 0; virtual const std::vector &get_gt_timestamps() const = 0; - virtual const Eigen::vector &get_gt_pose_data() const = 0; + virtual const Eigen::aligned_vector &get_gt_pose_data() + const = 0; virtual int64_t get_mocap_to_imu_offset_ns() const = 0; virtual std::vector get_image_data(int64_t t_ns) = 0; diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index 91daa08..fd3847c 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -57,11 +57,12 @@ class EurocVioDataset : public VioDataset { // missing frames // std::unordered_map> image_data; - Eigen::vector accel_data; - Eigen::vector gyro_data; + Eigen::aligned_vector accel_data; + Eigen::aligned_vector gyro_data; - std::vector gt_timestamps; // ordered gt timestamps - Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector gt_timestamps; // ordered gt timestamps + Eigen::aligned_vector + gt_pose_data; // TODO: change to eigen aligned int64_t mocap_to_imu_offset_ns = 0; @@ -74,12 +75,16 @@ class EurocVioDataset : public VioDataset { std::vector &get_image_timestamps() { return image_timestamps; } - const Eigen::vector &get_accel_data() const { return accel_data; } - const Eigen::vector &get_gyro_data() const { return gyro_data; } + const Eigen::aligned_vector &get_accel_data() const { + return accel_data; + } + const Eigen::aligned_vector &get_gyro_data() const { + return gyro_data; + } const std::vector &get_gt_timestamps() const { return gt_timestamps; } - const Eigen::vector &get_gt_pose_data() const { + const Eigen::aligned_vector &get_gt_pose_data() const { return gt_pose_data; } diff --git a/include/basalt/io/dataset_io_kitti.h b/include/basalt/io/dataset_io_kitti.h index 00ebeda..74ca418 100644 --- a/include/basalt/io/dataset_io_kitti.h +++ b/include/basalt/io/dataset_io_kitti.h @@ -57,11 +57,12 @@ class KittiVioDataset : public VioDataset { // missing frames // std::unordered_map> image_data; - Eigen::vector accel_data; - Eigen::vector gyro_data; + Eigen::aligned_vector accel_data; + Eigen::aligned_vector gyro_data; - std::vector gt_timestamps; // ordered gt timestamps - Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector gt_timestamps; // ordered gt timestamps + Eigen::aligned_vector + gt_pose_data; // TODO: change to eigen aligned int64_t mocap_to_imu_offset_ns; @@ -72,12 +73,16 @@ class KittiVioDataset : public VioDataset { std::vector &get_image_timestamps() { return image_timestamps; } - const Eigen::vector &get_accel_data() const { return accel_data; } - const Eigen::vector &get_gyro_data() const { return gyro_data; } + const Eigen::aligned_vector &get_accel_data() const { + return accel_data; + } + const Eigen::aligned_vector &get_gyro_data() const { + return gyro_data; + } const std::vector &get_gt_timestamps() const { return gt_timestamps; } - const Eigen::vector &get_gt_pose_data() const { + const Eigen::aligned_vector &get_gt_pose_data() const { return gt_pose_data; } int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } diff --git a/include/basalt/io/dataset_io_rosbag.h b/include/basalt/io/dataset_io_rosbag.h index 0a13ddb..638bf4b 100644 --- a/include/basalt/io/dataset_io_rosbag.h +++ b/include/basalt/io/dataset_io_rosbag.h @@ -71,11 +71,12 @@ class RosbagVioDataset : public VioDataset { std::unordered_map>> image_data_idx; - Eigen::vector accel_data; - Eigen::vector gyro_data; + Eigen::aligned_vector accel_data; + Eigen::aligned_vector gyro_data; - std::vector gt_timestamps; // ordered gt timestamps - Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector gt_timestamps; // ordered gt timestamps + Eigen::aligned_vector + gt_pose_data; // TODO: change to eigen aligned int64_t mocap_to_imu_offset_ns; @@ -86,12 +87,16 @@ class RosbagVioDataset : public VioDataset { std::vector &get_image_timestamps() { return image_timestamps; } - const Eigen::vector &get_accel_data() const { return accel_data; } - const Eigen::vector &get_gyro_data() const { return gyro_data; } + const Eigen::aligned_vector &get_accel_data() const { + return accel_data; + } + const Eigen::aligned_vector &get_gyro_data() const { + return gyro_data; + } const std::vector &get_gt_timestamps() const { return gt_timestamps; } - const Eigen::vector &get_gt_pose_data() const { + const Eigen::aligned_vector &get_gt_pose_data() const { return gt_pose_data; } diff --git a/include/basalt/io/dataset_io_uzh.h b/include/basalt/io/dataset_io_uzh.h index 77fbc2a..361c5b8 100644 --- a/include/basalt/io/dataset_io_uzh.h +++ b/include/basalt/io/dataset_io_uzh.h @@ -58,11 +58,12 @@ class UzhVioDataset : public VioDataset { // missing frames // std::unordered_map> image_data; - Eigen::vector accel_data; - Eigen::vector gyro_data; + Eigen::aligned_vector accel_data; + Eigen::aligned_vector gyro_data; - std::vector gt_timestamps; // ordered gt timestamps - Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector gt_timestamps; // ordered gt timestamps + Eigen::aligned_vector + gt_pose_data; // TODO: change to eigen aligned int64_t mocap_to_imu_offset_ns = 0; @@ -75,12 +76,16 @@ class UzhVioDataset : public VioDataset { std::vector &get_image_timestamps() { return image_timestamps; } - const Eigen::vector &get_accel_data() const { return accel_data; } - const Eigen::vector &get_gyro_data() const { return gyro_data; } + const Eigen::aligned_vector &get_accel_data() const { + return accel_data; + } + const Eigen::aligned_vector &get_gyro_data() const { + return gyro_data; + } const std::vector &get_gt_timestamps() const { return gt_timestamps; } - const Eigen::vector &get_gt_pose_data() const { + const Eigen::aligned_vector &get_gt_pose_data() const { return gt_pose_data; } diff --git a/include/basalt/optical_flow/frame_to_frame_optical_flow.h b/include/basalt/optical_flow/frame_to_frame_optical_flow.h index 34b32c2..ee46822 100644 --- a/include/basalt/optical_flow/frame_to_frame_optical_flow.h +++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h @@ -176,15 +176,16 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { frame_counter++; } - void trackPoints( - const basalt::ManagedImagePyr& pyr_1, - const basalt::ManagedImagePyr& pyr_2, - const Eigen::map& transform_map_1, - Eigen::map& transform_map_2) const { + void trackPoints(const basalt::ManagedImagePyr& pyr_1, + const basalt::ManagedImagePyr& pyr_2, + const Eigen::aligned_map& + transform_map_1, + Eigen::aligned_map& + transform_map_2) const { size_t num_points = transform_map_1.size(); std::vector ids; - Eigen::vector init_vec; + Eigen::aligned_vector init_vec; ids.reserve(num_points); init_vec.reserve(num_points); @@ -292,7 +293,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { } void addPoints() { - Eigen::vector pts0; + Eigen::aligned_vector pts0; for (const auto& kv : transforms->observations.at(0)) { pts0.emplace_back(kv.second.translation().cast()); @@ -303,7 +304,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { detectKeypoints(pyramid->at(0).lvl(0), kd, config.optical_flow_detection_grid_size, 1, pts0); - Eigen::map new_poses0, new_poses1; + Eigen::aligned_map new_poses0, + new_poses1; for (size_t i = 0; i < kd.corners.size(); i++) { Eigen::AffineCompact2f transform; @@ -331,7 +333,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { std::set lm_to_remove; std::vector kpid; - Eigen::vector proj0, proj1; + Eigen::aligned_vector proj0, proj1; for (const auto& kv : transforms->observations.at(1)) { auto it = transforms->observations.at(0).find(kv.first); @@ -343,7 +345,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { } } - Eigen::vector p3d0, p3d1; + Eigen::aligned_vector p3d0, p3d1; std::vector p3d0_success, p3d1_success; calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h index 929298c..d660199 100644 --- a/include/basalt/optical_flow/optical_flow.h +++ b/include/basalt/optical_flow/optical_flow.h @@ -62,7 +62,8 @@ struct OpticalFlowResult { using Ptr = std::shared_ptr; int64_t t_ns; - std::vector> observations; + std::vector> + observations; OpticalFlowInput::Ptr input_images; }; diff --git a/include/basalt/optical_flow/patch_optical_flow.h b/include/basalt/optical_flow/patch_optical_flow.h index 9469eb8..e5ed634 100644 --- a/include/basalt/optical_flow/patch_optical_flow.h +++ b/include/basalt/optical_flow/patch_optical_flow.h @@ -164,15 +164,16 @@ class PatchOpticalFlow : public OpticalFlowBase { frame_counter++; } - void trackPoints( - const basalt::ManagedImagePyr& pyr_1, - const basalt::ManagedImagePyr& pyr_2, - const Eigen::map& transform_map_1, - Eigen::map& transform_map_2) const { + void trackPoints(const basalt::ManagedImagePyr& pyr_1, + const basalt::ManagedImagePyr& pyr_2, + const Eigen::aligned_map& + transform_map_1, + Eigen::aligned_map& + transform_map_2) const { size_t num_points = transform_map_1.size(); std::vector ids; - Eigen::vector init_vec; + Eigen::aligned_vector init_vec; ids.reserve(num_points); init_vec.reserve(num_points); @@ -191,7 +192,7 @@ class PatchOpticalFlow : public OpticalFlowBase { const Eigen::AffineCompact2f& transform_1 = init_vec[r]; Eigen::AffineCompact2f transform_2 = transform_1; - const Eigen::vector& patch_vec = patches.at(id); + const Eigen::aligned_vector& patch_vec = patches.at(id); bool valid = trackPoint(pyr_2, patch_vec, transform_2); @@ -223,7 +224,7 @@ class PatchOpticalFlow : public OpticalFlowBase { } inline bool trackPoint(const basalt::ManagedImagePyr& pyr, - const Eigen::vector& patch_vec, + const Eigen::aligned_vector& patch_vec, Eigen::AffineCompact2f& transform) const { bool patch_valid = true; @@ -276,7 +277,7 @@ class PatchOpticalFlow : public OpticalFlowBase { } void addPoints() { - Eigen::vector pts0; + Eigen::aligned_vector pts0; for (const auto& kv : transforms->observations.at(0)) { pts0.emplace_back(kv.second.translation().cast()); @@ -287,10 +288,11 @@ class PatchOpticalFlow : public OpticalFlowBase { detectKeypoints(pyramid->at(0).lvl(0), kd, config.optical_flow_detection_grid_size, 1, pts0); - Eigen::map new_poses0, new_poses1; + Eigen::aligned_map new_poses0, + new_poses1; for (size_t i = 0; i < kd.corners.size(); i++) { - Eigen::vector& p = patches[last_keypoint_id]; + Eigen::aligned_vector& p = patches[last_keypoint_id]; Vector2 pos = kd.corners[i].cast(); @@ -325,7 +327,7 @@ class PatchOpticalFlow : public OpticalFlowBase { std::set lm_to_remove; std::vector kpid; - Eigen::vector proj0, proj1; + Eigen::aligned_vector proj0, proj1; for (const auto& kv : transforms->observations.at(1)) { auto it = transforms->observations.at(0).find(kv.first); @@ -337,7 +339,7 @@ class PatchOpticalFlow : public OpticalFlowBase { } } - Eigen::vector p3d0, p3d1; + Eigen::aligned_vector p3d0, p3d1; std::vector p3d0_success, p3d1_success; calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); @@ -372,7 +374,8 @@ class PatchOpticalFlow : public OpticalFlowBase { VioConfig config; basalt::Calibration calib; - Eigen::unordered_map> patches; + Eigen::aligned_unordered_map> + patches; OpticalFlowResult::Ptr transforms; std::shared_ptr>> old_pyramid, diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h index 3526708..ed06ea4 100644 --- a/include/basalt/optimization/linearize.h +++ b/include/basalt/optimization/linearize.h @@ -72,10 +72,11 @@ struct LinearizeBase { typedef Eigen::Matrix VectorX; typedef Eigen::Matrix MatrixX; - typedef typename Eigen::vector::const_iterator PoseDataIter; - typedef typename Eigen::vector::const_iterator GyroDataIter; - typedef typename Eigen::vector::const_iterator AccelDataIter; - typedef typename Eigen::vector::const_iterator + typedef typename Eigen::aligned_vector::const_iterator PoseDataIter; + typedef typename Eigen::aligned_vector::const_iterator GyroDataIter; + typedef + typename Eigen::aligned_vector::const_iterator AccelDataIter; + typedef typename Eigen::aligned_vector::const_iterator AprilgridCornersDataIter; template @@ -100,7 +101,8 @@ struct LinearizeBase { struct CalibCommonData { const Calibration* calibration = nullptr; const MocapCalibration* mocap_calibration = nullptr; - const Eigen::vector* aprilgrid_corner_pos_3d = nullptr; + const Eigen::aligned_vector* aprilgrid_corner_pos_3d = + nullptr; // Calib data const std::vector* offset_T_i_c = nullptr; diff --git a/include/basalt/optimization/poses_linearize.h b/include/basalt/optimization/poses_linearize.h index 78bf660..f7c9b47 100644 --- a/include/basalt/optimization/poses_linearize.h +++ b/include/basalt/optimization/poses_linearize.h @@ -63,7 +63,7 @@ struct LinearizePosesOpt : public LinearizeBase { typedef Eigen::Matrix VectorX; typedef Eigen::Matrix MatrixX; - typedef typename Eigen::vector::const_iterator + typedef typename Eigen::aligned_vector::const_iterator AprilgridCornersDataIter; typedef typename LinearizeBase::CalibCommonData CalibCommonData; @@ -75,11 +75,12 @@ struct LinearizePosesOpt : public LinearizeBase { size_t opt_size; - const Eigen::unordered_map& timestam_to_pose; + const Eigen::aligned_unordered_map& timestam_to_pose; - LinearizePosesOpt(size_t opt_size, - const Eigen::unordered_map& timestam_to_pose, - const CalibCommonData& common_data) + LinearizePosesOpt( + size_t opt_size, + const Eigen::aligned_unordered_map& timestam_to_pose, + const CalibCommonData& common_data) : opt_size(opt_size), timestam_to_pose(timestam_to_pose) { this->common_data = common_data; accum.reset(opt_size); @@ -191,7 +192,7 @@ struct ComputeErrorPosesOpt : public LinearizeBase { typedef Eigen::Matrix VectorX; typedef Eigen::Matrix MatrixX; - typedef typename Eigen::vector::const_iterator + typedef typename Eigen::aligned_vector::const_iterator AprilgridCornersDataIter; typedef typename LinearizeBase::CalibCommonData CalibCommonData; @@ -202,11 +203,11 @@ struct ComputeErrorPosesOpt : public LinearizeBase { size_t opt_size; - const Eigen::unordered_map& timestam_to_pose; + const Eigen::aligned_unordered_map& timestam_to_pose; ComputeErrorPosesOpt( size_t opt_size, - const Eigen::unordered_map& timestam_to_pose, + const Eigen::aligned_unordered_map& timestam_to_pose, const CalibCommonData& common_data) : opt_size(opt_size), timestam_to_pose(timestam_to_pose) { this->common_data = common_data; diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index 1fcfe9c..23cabdd 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/include/basalt/optimization/poses_optimize.h @@ -55,17 +55,17 @@ class PosesOptimization { using VectorX = typename LinearizeT::VectorX; using AprilgridCornersDataIter = - typename Eigen::vector::const_iterator; + typename Eigen::aligned_vector::const_iterator; public: PosesOptimization() : lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {} bool initializeIntrinsics( - size_t cam_id, const Eigen::vector &corners, + size_t cam_id, const Eigen::aligned_vector &corners, const std::vector &corner_ids, - const Eigen::vector &aprilgrid_corner_pos_3d, int cols, - int rows) { + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, + int cols, int rows) { Eigen::Vector4d init_intr; bool val = CalibHelper::initializeIntrinsics( @@ -156,8 +156,8 @@ class PosesOptimization { int max_iter = 10; while (!step && max_iter > 0 && !converged) { - Eigen::unordered_map timestam_to_pose_backup = - timestam_to_pose; + Eigen::aligned_unordered_map + timestam_to_pose_backup = timestam_to_pose; Calibration calib_backup = *calib; Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; @@ -170,12 +170,12 @@ class PosesOptimization { for (auto &kv : timestam_to_pose) { kv.second *= - Sophus::expd(inc.segment(offset_poses[kv.first])); + Sophus::se3_expd(inc.segment(offset_poses[kv.first])); } for (size_t i = 0; i < calib->T_i_c.size(); i++) { calib->T_i_c[i] *= - Sophus::expd(inc.segment(offset_T_i_c[i])); + Sophus::se3_expd(inc.segment(offset_T_i_c[i])); } for (size_t i = 0; i < calib->intrinsics.size(); i++) { @@ -264,7 +264,7 @@ class PosesOptimization { return it->second; } - void setAprilgridCorners3d(const Eigen::vector &v) { + void setAprilgridCorners3d(const Eigen::aligned_vector &v) { aprilgrid_corner_pos_3d = v; } @@ -280,7 +280,7 @@ class PosesOptimization { void addAprilgridMeasurement( int64_t t_ns, int cam_id, - const Eigen::vector &corners_pos, + const Eigen::aligned_vector &corners_pos, const std::vector &corner_id) { aprilgrid_corners_measurements.emplace_back(); @@ -298,7 +298,7 @@ class PosesOptimization { calib->vignette = vign; } - void setResolution(const Eigen::vector &resolution) { + void setResolution(const Eigen::aligned_vector &resolution) { calib->resolution = resolution; } @@ -319,11 +319,11 @@ class PosesOptimization { std::vector offset_T_i_c; // frame poses - Eigen::unordered_map timestam_to_pose; + Eigen::aligned_unordered_map timestam_to_pose; - Eigen::vector aprilgrid_corners_measurements; + Eigen::aligned_vector aprilgrid_corners_measurements; - Eigen::vector aprilgrid_corner_pos_3d; + Eigen::aligned_vector aprilgrid_corner_pos_3d; }; // namespace basalt diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h index f5d4f12..e9e67ab 100644 --- a/include/basalt/optimization/spline_linearize.h +++ b/include/basalt/optimization/spline_linearize.h @@ -80,13 +80,14 @@ struct LinearizeSplineOpt : public LinearizeBase { typedef Se3Spline SplineT; - typedef typename Eigen::deque::const_iterator PoseDataIter; - typedef typename Eigen::deque::const_iterator GyroDataIter; - typedef typename Eigen::deque::const_iterator AccelDataIter; - typedef typename Eigen::deque::const_iterator - AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator PoseDataIter; + typedef typename Eigen::aligned_deque::const_iterator GyroDataIter; typedef - typename Eigen::deque::const_iterator MocapPoseDataIter; + typename Eigen::aligned_deque::const_iterator AccelDataIter; + typedef typename Eigen::aligned_deque::const_iterator + AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator + MocapPoseDataIter; // typedef typename LinearizeBase::PoseCalibH PoseCalibH; typedef typename LinearizeBase::CalibCommonData CalibCommonData; @@ -519,7 +520,7 @@ struct LinearizeSplineOpt : public LinearizeBase { const SE3 T_mark_moc_meas = pm.data.inverse(); - Vector6 residual = Sophus::logd(T_mark_moc_meas * T_moc_mark); + Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark); // TODO: check derivatives Matrix6 d_res_d_T_i_mark; @@ -659,13 +660,14 @@ struct ComputeErrorSplineOpt : public LinearizeBase { typedef Se3Spline SplineT; - typedef typename Eigen::deque::const_iterator PoseDataIter; - typedef typename Eigen::deque::const_iterator GyroDataIter; - typedef typename Eigen::deque::const_iterator AccelDataIter; - typedef typename Eigen::deque::const_iterator - AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator PoseDataIter; + typedef typename Eigen::aligned_deque::const_iterator GyroDataIter; typedef - typename Eigen::deque::const_iterator MocapPoseDataIter; + typename Eigen::aligned_deque::const_iterator AccelDataIter; + typedef typename Eigen::aligned_deque::const_iterator + AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator + MocapPoseDataIter; // typedef typename LinearizeBase::PoseCalibH PoseCalibH; typedef typename LinearizeBase::CalibCommonData CalibCommonData; @@ -822,7 +824,7 @@ struct ComputeErrorSplineOpt : public LinearizeBase { const SE3 T_mark_moc_meas = pm.data.inverse(); - Vector6 residual = Sophus::logd(T_mark_moc_meas * T_moc_mark); + Vector6 residual = Sophus::se3_logd(T_mark_moc_meas * T_moc_mark); const Scalar& mocap_var_inv = this->common_data.mocap_var_inv; diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index 540eb11..36c648c 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -231,7 +231,7 @@ class SplineOptimization { SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); } - void setAprilgridCorners3d(const Eigen::vector& v) { + void setAprilgridCorners3d(const Eigen::aligned_vector& v) { aprilgrid_corner_pos_3d = v; } @@ -270,7 +270,7 @@ class SplineOptimization { void addAprilgridMeasurement( int64_t t_ns, int cam_id, - const Eigen::vector& corners_pos, + const Eigen::aligned_vector& corners_pos, const std::vector& corner_id) { min_time_us = std::min(min_time_us, t_ns); max_time_us = std::max(max_time_us, t_ns); @@ -519,13 +519,14 @@ class SplineOptimization { EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: - typedef typename Eigen::deque::const_iterator PoseDataIter; - typedef typename Eigen::deque::const_iterator GyroDataIter; - typedef typename Eigen::deque::const_iterator AccelDataIter; - typedef typename Eigen::deque::const_iterator - AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator PoseDataIter; + typedef typename Eigen::aligned_deque::const_iterator GyroDataIter; typedef - typename Eigen::deque::const_iterator MocapPoseDataIter; + typename Eigen::aligned_deque::const_iterator AccelDataIter; + typedef typename Eigen::aligned_deque::const_iterator + AprilgridCornersDataIter; + typedef typename Eigen::aligned_deque::const_iterator + MocapPoseDataIter; void applyInc(VectorX& inc_full, const std::vector& offset_cam_intrinsics) { @@ -550,7 +551,7 @@ class SplineOptimization { size_t T_i_c_block_offset = bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE; for (size_t i = 0; i < calib->T_i_c.size(); i++) { - calib->T_i_c[i] *= Sophus::expd(inc_full.template segment( + calib->T_i_c[i] *= Sophus::se3_expd(inc_full.template segment( T_i_c_block_offset + i * POSE_SIZE)); } @@ -561,9 +562,9 @@ class SplineOptimization { size_t mocap_block_offset = offset_cam_intrinsics.back(); - mocap_calib->T_moc_w *= - Sophus::expd(inc_full.template segment(mocap_block_offset)); - mocap_calib->T_i_mark *= Sophus::expd( + mocap_calib->T_moc_w *= Sophus::se3_expd( + inc_full.template segment(mocap_block_offset)); + mocap_calib->T_i_mark *= Sophus::se3_expd( inc_full.template segment(mocap_block_offset + POSE_SIZE)); mocap_calib->mocap_time_offset_ns += @@ -580,11 +581,11 @@ class SplineOptimization { int64_t min_time_us, max_time_us; - Eigen::deque pose_measurements; - Eigen::deque gyro_measurements; - Eigen::deque accel_measurements; - Eigen::deque aprilgrid_corners_measurements; - Eigen::deque mocap_measurements; + Eigen::aligned_deque pose_measurements; + Eigen::aligned_deque gyro_measurements; + Eigen::aligned_deque accel_measurements; + Eigen::aligned_deque aprilgrid_corners_measurements; + Eigen::aligned_deque mocap_measurements; typename LinearizeT::CalibCommonData ccd; @@ -597,7 +598,7 @@ class SplineOptimization { SplineT spline; Vector3 g; - Eigen::vector aprilgrid_corner_pos_3d; + Eigen::aligned_vector aprilgrid_corner_pos_3d; int64_t dt_ns; }; // namespace basalt diff --git a/include/basalt/utils/common_types.h b/include/basalt/utils/common_types.h index 72e41a8..ec0365e 100644 --- a/include/basalt/utils/common_types.h +++ b/include/basalt/utils/common_types.h @@ -102,7 +102,7 @@ struct KeypointsData { /// FeatureId) std::vector> corner_descriptors; - Eigen::vector corners_3d; + Eigen::aligned_vector corners_3d; std::vector hashes; HashBowVector bow_vector; diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index e0d2301..348db35 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -270,8 +270,8 @@ struct MargData { AbsOrderMap aom; Eigen::MatrixXd abs_H; Eigen::VectorXd abs_b; - Eigen::map frame_states; - Eigen::map frame_poses; + Eigen::aligned_map frame_states; + Eigen::aligned_map frame_poses; std::set kfs_all; std::set kfs_to_marg; bool use_imu; diff --git a/include/basalt/utils/keypoints.h b/include/basalt/utils/keypoints.h index dfd6c81..d9eed21 100644 --- a/include/basalt/utils/keypoints.h +++ b/include/basalt/utils/keypoints.h @@ -53,11 +53,11 @@ typedef std::bitset<256> Descriptor; void detectKeypointsMapping(const basalt::Image& img_raw, KeypointsData& kd, int num_features); -void detectKeypoints(const basalt::Image& img_raw, - KeypointsData& kd, int PATCH_SIZE = 32, - int num_points_cell = 1, - const Eigen::vector& current_points = - Eigen::vector()); +void detectKeypoints( + const basalt::Image& img_raw, KeypointsData& kd, + int PATCH_SIZE = 32, int num_points_cell = 1, + const Eigen::aligned_vector& current_points = + Eigen::aligned_vector()); void computeAngles(const basalt::Image& img_raw, KeypointsData& kd, bool rotate_features); diff --git a/include/basalt/utils/nfr.h b/include/basalt/utils/nfr.h index 8f8095f..7ea967b 100644 --- a/include/basalt/utils/nfr.h +++ b/include/basalt/utils/nfr.h @@ -45,7 +45,7 @@ inline Sophus::Vector6d relPoseError( const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr, Sophus::Matrix6d* d_res_d_T_w_j = nullptr) { Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i; - Sophus::Vector6d res = Sophus::logd(T_i_j * T_j_i); + Sophus::Vector6d res = Sophus::se3_logd(T_i_j * T_j_i); if (d_res_d_T_w_i || d_res_d_T_w_j) { Sophus::Matrix6d J; diff --git a/include/basalt/utils/sim_utils.h b/include/basalt/utils/sim_utils.h index 15b5806..1e8e98a 100644 --- a/include/basalt/utils/sim_utils.h +++ b/include/basalt/utils/sim_utils.h @@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { struct SimObservations { - Eigen::vector pos; + Eigen::aligned_vector pos; std::vector id; }; diff --git a/include/basalt/utils/vis_utils.h b/include/basalt/utils/vis_utils.h index 41c581e..fe2db28 100644 --- a/include/basalt/utils/vis_utils.h +++ b/include/basalt/utils/vis_utils.h @@ -50,7 +50,7 @@ inline void render_camera(const Eigen::Matrix4d& T_w_c, float lineWidth, const float sz = sizeFactor; const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240; - const Eigen::vector lines = { + const Eigen::aligned_vector lines = { {0, 0, 0}, {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz}, {0, 0, 0}, diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 8db8e1e..2cb0d4b 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -45,8 +45,8 @@ class BundleAdjustmentBase { struct RelLinDataBase { std::vector> order; - Eigen::vector d_rel_d_h; - Eigen::vector d_rel_d_t; + Eigen::aligned_vector d_rel_d_h; + Eigen::aligned_vector d_rel_d_t; }; struct FrameRelLinData { @@ -54,7 +54,7 @@ class BundleAdjustmentBase { Sophus::Vector6d bp; std::vector lm_id; - Eigen::vector> Hpl; + Eigen::aligned_vector> Hpl; FrameRelLinData() { Hpp.setZero(); @@ -88,11 +88,12 @@ class BundleAdjustmentBase { } } - Eigen::unordered_map Hll; - Eigen::unordered_map bl; - Eigen::unordered_map>> lm_to_obs; + Eigen::aligned_unordered_map Hll; + Eigen::aligned_unordered_map bl; + Eigen::aligned_unordered_map>> + lm_to_obs; - Eigen::vector Hpppl; + Eigen::aligned_vector Hpppl; double error; }; @@ -103,9 +104,11 @@ class BundleAdjustmentBase { double outlier_threshold = 0) const; void linearizeHelper( - Eigen::vector& rld_vec, - const Eigen::map< - TimeCamId, Eigen::map>>& + Eigen::aligned_vector& rld_vec, + const Eigen::aligned_map< + TimeCamId, + Eigen::aligned_map>>& obs_to_lin, double& error) const; @@ -227,7 +230,7 @@ class BundleAdjustmentBase { Sophus::Matrix6d* d_rel_d_h = nullptr, Sophus::Matrix6d* d_rel_d_t = nullptr); - void get_current_points(Eigen::vector& points, + void get_current_points(Eigen::aligned_vector& points, std::vector& ids) const; // Modifies abs_H and abs_b as a side effect. @@ -254,8 +257,8 @@ class BundleAdjustmentBase { static Eigen::VectorXd checkNullspace( const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b, const AbsOrderMap& marg_order, - const Eigen::map& frame_states, - const Eigen::map& frame_poses); + const Eigen::aligned_map& frame_states, + const Eigen::aligned_map& frame_poses); /// Triangulates the point and returns homogenous representation. First 3 /// components - unit-length direction vector. Last component inverse @@ -359,7 +362,7 @@ class BundleAdjustmentBase { template struct LinearizeAbsReduce { - using RelLinDataIter = Eigen::vector::iterator; + using RelLinDataIter = Eigen::aligned_vector::iterator; LinearizeAbsReduce(AbsOrderMap& aom) : aom(aom) { accum.reset(aom.total_size); @@ -414,8 +417,8 @@ class BundleAdjustmentBase { return PoseStateWithLin(it2->second); } - Eigen::map frame_states; - Eigen::map frame_poses; + Eigen::aligned_map frame_states; + Eigen::aligned_map frame_poses; // Point management LandmarkDatabase lmdb; diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h index 2e9f13b..e4151e5 100644 --- a/include/basalt/vi_estimator/keypoint_vio.h +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -84,16 +84,16 @@ class KeypointVioEstimator : public VioEstimatorBase, static void linearizeAbsIMU( const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, double& imu_error, double& bg_error, double& ba_error, - const Eigen::map& states, - const Eigen::map& imu_meas, + const Eigen::aligned_map& states, + const Eigen::aligned_map& imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); static void computeImuError( const AbsOrderMap& aom, double& imu_error, double& bg_error, double& ba_error, - const Eigen::map& states, - const Eigen::map& imu_meas, + const Eigen::aligned_map& states, + const Eigen::aligned_map& imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); @@ -138,13 +138,13 @@ class KeypointVioEstimator : public VioEstimatorBase, // const MatNN get_cov() const { return cov.bottomRightCorner(); } void computeProjections( - std::vector>& res) const; + std::vector>& res) const; inline void setMaxStates(size_t val) { max_states = val; } inline void setMaxKfs(size_t val) { max_kfs = val; } - Eigen::vector getFrameStates() const { - Eigen::vector res; + Eigen::aligned_vector getFrameStates() const { + Eigen::aligned_vector res; for (const auto& kv : frame_states) { res.push_back(kv.second.getState().T_w_i); @@ -153,8 +153,8 @@ class KeypointVioEstimator : public VioEstimatorBase, return res; } - Eigen::vector getFramePoses() const { - Eigen::vector res; + Eigen::aligned_vector getFramePoses() const { + Eigen::aligned_vector res; for (const auto& kv : frame_poses) { res.push_back(kv.second.getPose()); @@ -163,8 +163,8 @@ class KeypointVioEstimator : public VioEstimatorBase, return res; } - Eigen::map getAllPosesMap() const { - Eigen::map res; + Eigen::aligned_map getAllPosesMap() const { + Eigen::aligned_map res; for (const auto& kv : frame_poses) { res[kv.first] = kv.second.getPose(); @@ -187,13 +187,13 @@ class KeypointVioEstimator : public VioEstimatorBase, std::set kf_ids; int64_t last_state_t_ns; - Eigen::map imu_meas; + Eigen::aligned_map imu_meas; const Eigen::Vector3d g; // Input - Eigen::map prev_opt_flow_res; + Eigen::aligned_map prev_opt_flow_res; std::map num_points_kf; diff --git a/include/basalt/vi_estimator/keypoint_vo.h b/include/basalt/vi_estimator/keypoint_vo.h index d3c1afe..4769b2f 100644 --- a/include/basalt/vi_estimator/keypoint_vo.h +++ b/include/basalt/vi_estimator/keypoint_vo.h @@ -120,13 +120,13 @@ class KeypointVoEstimator : public VioEstimatorBase, // const MatNN get_cov() const { return cov.bottomRightCorner(); } void computeProjections( - std::vector>& res) const; + std::vector>& res) const; inline void setMaxStates(size_t val) { max_states = val; } inline void setMaxKfs(size_t val) { max_kfs = val; } - Eigen::vector getFrameStates() const { - Eigen::vector res; + Eigen::aligned_vector getFrameStates() const { + Eigen::aligned_vector res; for (const auto& kv : frame_states) { res.push_back(kv.second.getState().T_w_i); @@ -135,8 +135,8 @@ class KeypointVoEstimator : public VioEstimatorBase, return res; } - Eigen::vector getFramePoses() const { - Eigen::vector res; + Eigen::aligned_vector getFramePoses() const { + Eigen::aligned_vector res; for (const auto& kv : frame_poses) { res.push_back(kv.second.getPose()); @@ -145,8 +145,8 @@ class KeypointVoEstimator : public VioEstimatorBase, return res; } - Eigen::map getAllPosesMap() const { - Eigen::map res; + Eigen::aligned_map getAllPosesMap() const { + Eigen::aligned_map res; for (const auto& kv : frame_poses) { res[kv.first] = kv.second.getPose(); @@ -172,7 +172,7 @@ class KeypointVoEstimator : public VioEstimatorBase, // Input - Eigen::map prev_opt_flow_res; + Eigen::aligned_map prev_opt_flow_res; std::map num_points_kf; diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h index 69cd42b..692d095 100644 --- a/include/basalt/vi_estimator/landmark_database.h +++ b/include/basalt/vi_estimator/landmark_database.h @@ -92,8 +92,9 @@ class LandmarkDatabase { std::vector getLandmarksForHost( const TimeCamId& tcid) const; - const Eigen::map>>& + const Eigen::aligned_map< + TimeCamId, Eigen::aligned_map< + TimeCamId, Eigen::aligned_vector>>& getObservations() const; bool landmarkExists(int lm_id) const; @@ -117,15 +118,16 @@ class LandmarkDatabase { } private: - Eigen::unordered_map kpts; - Eigen::map>> + Eigen::aligned_unordered_map kpts; + Eigen::aligned_map< + TimeCamId, + Eigen::aligned_map>> obs; std::unordered_map> host_to_kpts; int num_observations = 0; - Eigen::unordered_map kpts_num_obs; + Eigen::aligned_unordered_map kpts_num_obs; }; } // namespace basalt diff --git a/include/basalt/vi_estimator/nfr_mapper.h b/include/basalt/vi_estimator/nfr_mapper.h index e939ffa..2781cc3 100644 --- a/include/basalt/vi_estimator/nfr_mapper.h +++ b/include/basalt/vi_estimator/nfr_mapper.h @@ -68,13 +68,14 @@ class NfrMapper : public BundleAdjustmentBase { struct MapperLinearizeAbsReduce : public BundleAdjustmentBase::LinearizeAbsReduce { using RollPitchFactorConstIter = - Eigen::vector::const_iterator; - using RelPoseFactorConstIter = Eigen::vector::const_iterator; - using RelLinDataIter = Eigen::vector::iterator; + Eigen::aligned_vector::const_iterator; + using RelPoseFactorConstIter = + Eigen::aligned_vector::const_iterator; + using RelLinDataIter = Eigen::aligned_vector::iterator; MapperLinearizeAbsReduce( AbsOrderMap& aom, - const Eigen::map* frame_poses) + const Eigen::aligned_map* frame_poses) : BundleAdjustmentBase::LinearizeAbsReduce(aom), frame_poses(frame_poses) { this->accum.reset(aom.total_size); @@ -159,7 +160,7 @@ class NfrMapper : public BundleAdjustmentBase { double roll_pitch_error; double rel_error; - const Eigen::map* frame_poses; + const Eigen::aligned_map* frame_poses; }; NfrMapper(const basalt::Calibration& calib, const VioConfig& config); @@ -172,7 +173,7 @@ class NfrMapper : public BundleAdjustmentBase { void optimize(int num_iterations = 10); - Eigen::map& getFramePoses(); + Eigen::aligned_map& getFramePoses(); void computeRelPose(double& rel_error); @@ -191,8 +192,8 @@ class NfrMapper : public BundleAdjustmentBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::vector roll_pitch_factors; - Eigen::vector rel_pose_factors; + Eigen::aligned_vector roll_pitch_factors; + Eigen::aligned_vector rel_pose_factors; std::unordered_map img_data; diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h index 6f67ab5..48a19a0 100644 --- a/include/basalt/vi_estimator/vio_estimator.h +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -46,15 +46,15 @@ struct VioVisualizationData { int64_t t_ns; - Eigen::vector states; - Eigen::vector frames; + Eigen::aligned_vector states; + Eigen::aligned_vector frames; - Eigen::vector points; + Eigen::aligned_vector points; std::vector point_ids; OpticalFlowResult::Ptr opt_flow_res; - std::vector> projections; + std::vector> projections; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -105,7 +105,7 @@ class VioEstimatorFactory { }; double alignSVD(const std::vector& filter_t_ns, - const Eigen::vector& filter_t_w_i, + const Eigen::aligned_vector& filter_t_w_i, const std::vector& gt_t_ns, - Eigen::vector& gt_t_w_i); + Eigen::aligned_vector& gt_t_w_i); } // namespace basalt diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp index f66d3e3..c4cfe61 100644 --- a/src/calibration/calibraiton_helper.cpp +++ b/src/calibration/calibraiton_helper.cpp @@ -51,9 +51,10 @@ namespace basalt { template bool estimateTransformation( - const CamT &cam_calib, const Eigen::vector &corners, + const CamT &cam_calib, + const Eigen::aligned_vector &corners, const std::vector &corner_ids, - const Eigen::vector &aprilgrid_corner_pos_3d, + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, Sophus::SE3d &T_target_camera, size_t &num_inliers) { opengv::bearingVectors_t bearingVectors; opengv::points_t points; @@ -140,7 +141,7 @@ void CalibHelper::detectCorners( void CalibHelper::initCamPoses( const Calibration::Ptr &calib, - const Eigen::vector &aprilgrid_corner_pos_3d, + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, tbb::concurrent_unordered_map &calib_corners, tbb::concurrent_unordered_map &calib_init_poses) { @@ -169,13 +170,13 @@ void CalibHelper::initCamPoses( } bool CalibHelper::initializeIntrinsics( - const Eigen::vector &corners, + const Eigen::aligned_vector &corners, const std::vector &corner_ids, - const Eigen::vector &aprilgrid_corner_pos_3d, int cols, - int rows, Eigen::Vector4d &init_intr) { + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, + int cols, int rows, Eigen::Vector4d &init_intr) { // First, initialize the image center at the center of the image. - Eigen::map id_to_corner; + Eigen::aligned_map id_to_corner; for (size_t i = 0; i < corner_ids.size(); i++) { id_to_corner[corner_ids[i]] = corners[i]; } @@ -197,7 +198,7 @@ bool CalibHelper::initializeIntrinsics( for (size_t r = 0; r < target_rows; ++r) { // cv::Mat P(target.cols(); 4, CV_64F); - Eigen::vector P; + Eigen::aligned_vector P; for (size_t c = 0; c < target_cols; ++c) { int tag_offset = (r * target_cols + c) << 2; @@ -302,7 +303,7 @@ bool CalibHelper::initializeIntrinsics( void CalibHelper::computeInitialPose( const Calibration::Ptr &calib, size_t cam_id, - const Eigen::vector &aprilgrid_corner_pos_3d, + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, const CalibCornerData &cd, CalibInitPoseData &cp) { if (cd.corners.size() < 8) { cp.num_inliers = 0; @@ -336,9 +337,9 @@ void CalibHelper::computeInitialPose( size_t CalibHelper::computeReprojectionError( const UnifiedCamera &cam_calib, - const Eigen::vector &corners, + const Eigen::aligned_vector &corners, const std::vector &corner_ids, - const Eigen::vector &aprilgrid_corner_pos_3d, + const Eigen::aligned_vector &aprilgrid_corner_pos_3d, const Sophus::SE3d &T_target_camera, double &error) { size_t num_projected = 0; error = 0; diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index 3822cb0..8eb2c5d 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -154,13 +154,14 @@ void CamCalib::initGui() { } void CamCalib::computeVign() { - Eigen::vector optical_centers; + Eigen::aligned_vector optical_centers; for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { optical_centers.emplace_back( calib_opt->calib->intrinsics[i].getParam().segment<2>(2)); } - std::map> reprojected_vignette2; + std::map> + reprojected_vignette2; for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; const std::vector img_vec = @@ -172,7 +173,7 @@ void CamCalib::computeVign() { auto it = reprojected_vignette.find(tcid); if (it != reprojected_vignette.end() && img_vec[j].img.get()) { - Eigen::vector rv; + Eigen::aligned_vector rv; rv.resize(it->second.corners_proj.size()); for (size_t k = 0; k < it->second.corners_proj.size(); k++) { @@ -314,7 +315,7 @@ void CamCalib::computeProjections() { TimeCamId tcid(timestamp_ns, i); ProjectedCornerData rc, rv; - Eigen::vector polar_azimuthal_angle; + Eigen::aligned_vector polar_azimuthal_angle; Sophus::SE3d T_c_w_ = (calib_opt->getT_w_i(timestamp_ns) * calib_opt->calib->T_i_c[i]) @@ -498,7 +499,7 @@ void CamCalib::initCamIntrinsics() { } } - Eigen::vector res; + Eigen::aligned_vector res; for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { res.emplace_back(img_data[i].img->w, img_data[i].img->h); diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index f39f3b7..94b5262 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -313,8 +313,8 @@ void CamImuCalib::initCamImuTransform() { } std::vector timestamps_cam; - Eigen::vector rot_vel_cam; - Eigen::vector rot_vel_imu; + Eigen::aligned_vector rot_vel_cam; + Eigen::aligned_vector rot_vel_imu; Sophus::SO3d R_i_c0_init = calib_opt->getCamT_i_c(0).so3(); @@ -492,8 +492,8 @@ void CamImuCalib::initMocap() { { std::vector timestamps_cam; - Eigen::vector rot_vel_mocap; - Eigen::vector rot_vel_imu; + Eigen::aligned_vector rot_vel_mocap; + Eigen::aligned_vector rot_vel_imu; Sophus::SO3d R_i_mark_init = calib_opt->mocap_calib->T_i_mark.so3(); diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp index 1729e5d..814c256 100644 --- a/src/calibration/vignette.cpp +++ b/src/calibration/vignette.cpp @@ -41,9 +41,9 @@ namespace basalt { VignetteEstimator::VignetteEstimator( const VioDatasetPtr &vio_dataset, - const Eigen::vector &optical_centers, - const Eigen::vector &resolutions, - const std::map> + const Eigen::aligned_vector &optical_centers, + const Eigen::aligned_vector &resolutions, + const std::map> &reprojected_vignette, const AprilGrid &april_grid) : vio_dataset(vio_dataset), diff --git a/src/kitti_eval.cpp b/src/kitti_eval.cpp index a90845f..49c700a 100644 --- a/src/kitti_eval.cpp +++ b/src/kitti_eval.cpp @@ -66,8 +66,8 @@ inline void load(Archive& ar, std::map& map) { } // namespace cereal -Eigen::vector load_poses(const std::string& path) { - Eigen::vector res; +Eigen::aligned_vector load_poses(const std::string& path) { + Eigen::aligned_vector res; std::ifstream f(path); std::string line; @@ -90,8 +90,8 @@ Eigen::vector load_poses(const std::string& path) { } void eval_kitti(const std::vector& lengths, - const Eigen::vector& poses_gt, - const Eigen::vector& poses_result, + const Eigen::aligned_vector& poses_gt, + const Eigen::aligned_vector& poses_result, std::map>& res) { auto lastFrameFromSegmentLength = [](std::vector& dist, int first_frame, float len) { @@ -185,8 +185,9 @@ int main(int argc, char** argv) { return app.exit(e); } - const Eigen::vector poses_gt = load_poses(gt_path); - const Eigen::vector poses_result = load_poses(traj_path); + const Eigen::aligned_vector poses_gt = load_poses(gt_path); + const Eigen::aligned_vector poses_result = + load_poses(traj_path); if (poses_gt.empty() || poses_gt.size() != poses_result.size()) { std::cerr << "Wrong number of poses: poses_gt " << poses_gt.size() diff --git a/src/mapper.cpp b/src/mapper.cpp index 4be1069..a214d67 100644 --- a/src/mapper.cpp +++ b/src/mapper.cpp @@ -74,23 +74,23 @@ using basalt::POSE_VEL_BIAS_SIZE; Eigen::Vector3d g(0, 0, -9.81); -const Eigen::vector image_resolutions = {{752, 480}, - {752, 480}}; +const Eigen::aligned_vector image_resolutions = {{752, 480}, + {752, 480}}; basalt::VioConfig vio_config; basalt::NfrMapper::Ptr nrf_mapper; -Eigen::vector gt_frame_t_w_i; +Eigen::aligned_vector gt_frame_t_w_i; std::vector gt_frame_t_ns, image_t_ns; -Eigen::vector mapper_points; +Eigen::aligned_vector mapper_points; std::vector mapper_point_ids; std::map marg_data; -Eigen::vector edges_vis; -Eigen::vector roll_pitch_vis; -Eigen::vector rel_edges_vis; +Eigen::aligned_vector edges_vis; +Eigen::aligned_vector roll_pitch_vis; +Eigen::aligned_vector rel_edges_vis; void draw_image_overlay(pangolin::View& v, size_t cam_id); void draw_scene(); @@ -642,7 +642,7 @@ void optimize() { } double alignButton() { - Eigen::vector filter_t_w_i; + Eigen::aligned_vector filter_t_w_i; std::vector filter_t_ns; for (const auto& kv : nrf_mapper->getFramePoses()) { diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp index 79190d6..0645f8f 100644 --- a/src/mapper_sim.cpp +++ b/src/mapper_sim.cpp @@ -77,28 +77,28 @@ Eigen::Vector3d g(0, 0, -9.81); std::shared_ptr> gt_spline; -Eigen::vector gt_frame_T_w_i; -Eigen::vector gt_frame_t_w_i, vio_t_w_i; +Eigen::aligned_vector gt_frame_T_w_i; +Eigen::aligned_vector gt_frame_t_w_i, vio_t_w_i; std::vector gt_frame_t_ns; -Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, - noisy_accel, noisy_gyro, gt_vel; +Eigen::aligned_vector gt_accel, gt_gyro, gt_accel_bias, + gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel; std::vector gt_imu_t_ns; -Eigen::vector filter_points; +Eigen::aligned_vector filter_points; std::vector filter_point_ids; std::map marg_data; -Eigen::vector roll_pitch_factors; -Eigen::vector rel_pose_factors; +Eigen::aligned_vector roll_pitch_factors; +Eigen::aligned_vector rel_pose_factors; -Eigen::vector edges_vis; -Eigen::vector roll_pitch_vis; -Eigen::vector rel_edges_vis; +Eigen::aligned_vector edges_vis; +Eigen::aligned_vector roll_pitch_vis; +Eigen::aligned_vector rel_edges_vis; -Eigen::vector mapper_points; +Eigen::aligned_vector mapper_points; std::vector mapper_point_ids; basalt::NfrMapper::Ptr nrf_mapper; @@ -282,7 +282,7 @@ void load_data(const std::string& calib_path, const std::string& cache_path) { cereal::JSONInputArchive archive(is); int64_t t_ns; - Eigen::vector knots; + Eigen::aligned_vector knots; archive(cereal::make_nvp("t_ns", t_ns)); archive(cereal::make_nvp("knots", knots)); @@ -532,7 +532,7 @@ void optimize() { void randomInc() { Sophus::Vector6d rnd = Sophus::Vector6d::Random().array().abs(); - Sophus::SE3d random_inc = Sophus::expd(rnd / 10); + Sophus::SE3d random_inc = Sophus::se3_expd(rnd / 10); for (auto& kv : nrf_mapper->getFramePoses()) { Sophus::SE3d pose = random_inc * kv.second.getPose(); @@ -548,7 +548,7 @@ void randomYawInc() { rnd.setZero(); rnd[5] = std::abs(Eigen::Vector2d::Random()[0]); - Sophus::SE3d random_inc = Sophus::expd(rnd); + Sophus::SE3d random_inc = Sophus::se3_expd(rnd); std::cout << "random_inc\n" << random_inc.matrix() << std::endl; @@ -562,7 +562,7 @@ void randomYawInc() { } double alignButton() { - Eigen::vector filter_t_w_i; + Eigen::aligned_vector filter_t_w_i; std::vector filter_t_ns; for (const auto& kv : nrf_mapper->getFramePoses()) { diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index d992056..8fad8bc 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -88,12 +88,12 @@ std::mt19937 gen{1}; basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); -Eigen::vector gt_points; -Eigen::vector gt_frame_T_w_i; -Eigen::vector gt_frame_t_w_i, vio_t_w_i; +Eigen::aligned_vector gt_points; +Eigen::aligned_vector gt_frame_T_w_i; +Eigen::aligned_vector gt_frame_t_w_i, vio_t_w_i; std::vector gt_frame_t_ns, kf_t_ns; -Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, - noisy_accel, noisy_gyro, gt_vel; +Eigen::aligned_vector gt_accel, gt_gyro, gt_accel_bias, + gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel; std::vector gt_imu_t_ns; std::map gt_observations; @@ -356,7 +356,7 @@ int main(int argc, char** argv) { // t4.join(); if (!result_path.empty()) { - Eigen::vector vio_t_w_i; + Eigen::aligned_vector vio_t_w_i; auto it = vis_map.find(kf_t_ns.back()); @@ -528,7 +528,7 @@ void gen_data() { cereal::JSONInputArchive archive(is); int64_t t_ns; - Eigen::vector knots; + Eigen::aligned_vector knots; archive(cereal::make_nvp("t_ns", t_ns)); archive(cereal::make_nvp("knots", knots)); @@ -571,7 +571,7 @@ void gen_data() { mdl.start(marg_data_path); - Eigen::map tmp_poses; + Eigen::aligned_map tmp_poses; while (true) { basalt::MargData::Ptr data; @@ -747,7 +747,7 @@ bool next_step() { } void alignButton() { - Eigen::vector vio_t_w_i; + Eigen::aligned_vector vio_t_w_i; auto it = vis_map.find(kf_t_ns.back()); diff --git a/src/opt_flow.cpp b/src/opt_flow.cpp index 29c05c5..b824db2 100644 --- a/src/opt_flow.cpp +++ b/src/opt_flow.cpp @@ -293,8 +293,8 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (observations.count(t_ns) > 0) { - const Eigen::map& kp_map = - observations.at(t_ns)->observations[cam_id]; + const Eigen::aligned_map& + kp_map = observations.at(t_ns)->observations[cam_id]; for (const auto& kv : kp_map) { Eigen::MatrixXf transformed_patch = diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 1cdd382..9a176a7 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -100,7 +100,7 @@ tbb::concurrent_bounded_queue out_vis_queue; tbb::concurrent_bounded_queue out_state_queue; std::vector vio_t_ns; -Eigen::vector vio_t_w_i; +Eigen::aligned_vector vio_t_w_i; std::string marg_data_path; @@ -408,7 +408,8 @@ void draw_scene() { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glColor3ubv(cam_color); - Eigen::vector sub_gt(vio_t_w_i.begin(), vio_t_w_i.end()); + Eigen::aligned_vector sub_gt(vio_t_w_i.begin(), + vio_t_w_i.end()); pangolin::glDrawLineStrip(sub_gt); if (curr_vis_data.get()) { diff --git a/src/time_alignment.cpp b/src/time_alignment.cpp index 5ec0028..588e6e2 100644 --- a/src/time_alignment.cpp +++ b/src/time_alignment.cpp @@ -55,11 +55,11 @@ basalt::Calibration calib; basalt::MocapCalibration mocap_calib; // Linear time version -double compute_error(int64_t offset, - const std::vector &gyro_timestamps, - const Eigen::vector &gyro_data, - const std::vector &mocap_rot_vel_timestamps, - const Eigen::vector &mocap_rot_vel_data) { +double compute_error( + int64_t offset, const std::vector &gyro_timestamps, + const Eigen::aligned_vector &gyro_data, + const std::vector &mocap_rot_vel_timestamps, + const Eigen::aligned_vector &mocap_rot_vel_data) { double error = 0; int num_points = 0; @@ -177,10 +177,10 @@ int main(int argc, char **argv) { vio_dataset = dataset_io->get_data(); std::vector gyro_timestamps; - Eigen::vector gyro_data; + Eigen::aligned_vector gyro_data; std::vector mocap_rot_vel_timestamps; - Eigen::vector mocap_rot_vel_data; + Eigen::aligned_vector mocap_rot_vel_data; // Apply calibration to gyro { diff --git a/src/utils/keypoints.cpp b/src/utils/keypoints.cpp index 2724233..0256a02 100644 --- a/src/utils/keypoints.cpp +++ b/src/utils/keypoints.cpp @@ -154,9 +154,10 @@ void detectKeypointsMapping(const basalt::Image& img_raw, } } -void detectKeypoints(const basalt::Image& img_raw, - KeypointsData& kd, int PATCH_SIZE, int num_points_cell, - const Eigen::vector& current_points) { +void detectKeypoints( + const basalt::Image& img_raw, KeypointsData& kd, + int PATCH_SIZE, int num_points_cell, + const Eigen::aligned_vector& current_points) { kd.corners.clear(); kd.corner_angles.clear(); kd.corner_descriptors.clear(); diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 06940aa..bd56b03 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -227,9 +227,10 @@ void BundleAdjustmentBase::computeError( } void BundleAdjustmentBase::linearizeHelper( - Eigen::vector& rld_vec, - const Eigen::map>>& + Eigen::aligned_vector& rld_vec, + const Eigen::aligned_map< + TimeCamId, Eigen::aligned_map< + TimeCamId, Eigen::aligned_vector>>& obs_to_lin, double& error) const { error = 0; @@ -422,7 +423,8 @@ void BundleAdjustmentBase::linearizeRel(const RelLinData& rld, } void BundleAdjustmentBase::get_current_points( - Eigen::vector& points, std::vector& ids) const { + Eigen::aligned_vector& points, + std::vector& ids) const { points.clear(); ids.clear(); @@ -632,8 +634,8 @@ void BundleAdjustmentBase::computeMargPriorError( Eigen::VectorXd BundleAdjustmentBase::checkNullspace( const Eigen::MatrixXd& H, const Eigen::VectorXd& b, const AbsOrderMap& order, - const Eigen::map& frame_states, - const Eigen::map& frame_poses) { + const Eigen::aligned_map& frame_states, + const Eigen::aligned_map& frame_poses) { BASALT_ASSERT(size_t(H.cols()) == order.total_size); size_t marg_size = order.total_size; diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index fb4f21b..87b11b9 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -573,8 +573,9 @@ void KeypointVioEstimator::marginalize( { // Linearize points - Eigen::map>> + Eigen::aligned_map< + TimeCamId, Eigen::aligned_map< + TimeCamId, Eigen::aligned_vector>> obs_to_lin; for (auto it = lmdb.getObservations().cbegin(); @@ -590,7 +591,7 @@ void KeypointVioEstimator::marginalize( } double rld_error; - Eigen::vector rld_vec; + Eigen::aligned_vector rld_vec; linearizeHelper(rld_vec, obs_to_lin, rld_error); @@ -791,13 +792,13 @@ void KeypointVioEstimator::optimize() { auto t1 = std::chrono::high_resolution_clock::now(); double rld_error; - Eigen::vector rld_vec; + Eigen::aligned_vector rld_vec; linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); BundleAdjustmentBase::LinearizeAbsReduce> lopt( aom); - tbb::blocked_range::iterator> range( + tbb::blocked_range::iterator> range( rld_vec.begin(), rld_vec.end()); tbb::parallel_reduce(range, lopt); @@ -998,7 +999,7 @@ void KeypointVioEstimator::optimize() { } // namespace basalt void KeypointVioEstimator::computeProjections( - std::vector>& data) const { + std::vector>& data) const { for (const auto& kv : lmdb.getObservations()) { const TimeCamId& tcid_h = kv.first; diff --git a/src/vi_estimator/keypoint_vio_linearize.cpp b/src/vi_estimator/keypoint_vio_linearize.cpp index 52ccdb0..d62b036 100644 --- a/src/vi_estimator/keypoint_vio_linearize.cpp +++ b/src/vi_estimator/keypoint_vio_linearize.cpp @@ -40,8 +40,8 @@ namespace basalt { void KeypointVioEstimator::linearizeAbsIMU( const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, double& imu_error, double& bg_error, double& ba_error, - const Eigen::map& states, - const Eigen::map& imu_meas, + const Eigen::aligned_map& states, + const Eigen::aligned_map& imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { imu_error = 0; @@ -179,8 +179,8 @@ void KeypointVioEstimator::linearizeAbsIMU( void KeypointVioEstimator::computeImuError( const AbsOrderMap& aom, double& imu_error, double& bg_error, double& ba_error, - const Eigen::map& states, - const Eigen::map& imu_meas, + const Eigen::aligned_map& states, + const Eigen::aligned_map& imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { imu_error = 0; diff --git a/src/vi_estimator/keypoint_vo.cpp b/src/vi_estimator/keypoint_vo.cpp index 7e57142..a21f317 100644 --- a/src/vi_estimator/keypoint_vo.cpp +++ b/src/vi_estimator/keypoint_vo.cpp @@ -487,8 +487,10 @@ void KeypointVoEstimator::marginalize( { // Linearize points - Eigen::map>> + Eigen::aligned_map< + TimeCamId, + Eigen::aligned_map>> obs_to_lin; for (auto it = lmdb.getObservations().cbegin(); @@ -503,7 +505,7 @@ void KeypointVoEstimator::marginalize( } double rld_error; - Eigen::vector rld_vec; + Eigen::aligned_vector rld_vec; linearizeHelper(rld_vec, obs_to_lin, rld_error); @@ -651,13 +653,13 @@ void KeypointVoEstimator::optimize() { auto t1 = std::chrono::high_resolution_clock::now(); double rld_error; - Eigen::vector rld_vec; + Eigen::aligned_vector rld_vec; linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); BundleAdjustmentBase::LinearizeAbsReduce> lopt( aom); - tbb::blocked_range::iterator> range( + tbb::blocked_range::iterator> range( rld_vec.begin(), rld_vec.end()); tbb::parallel_reduce(range, lopt); @@ -836,7 +838,7 @@ void KeypointVoEstimator::optimize() { } // namespace basalt void KeypointVoEstimator::computeProjections( - std::vector>& data) const { + std::vector>& data) const { for (const auto& kv : lmdb.getObservations()) { const TimeCamId& tcid_h = kv.first; diff --git a/src/vi_estimator/landmark_database.cpp b/src/vi_estimator/landmark_database.cpp index f1f42ce..a7f9db9 100644 --- a/src/vi_estimator/landmark_database.cpp +++ b/src/vi_estimator/landmark_database.cpp @@ -145,8 +145,9 @@ const KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) const { return kpts.at(lm_id); } -const Eigen::map > > +const Eigen::aligned_map< + TimeCamId, + Eigen::aligned_map > > &LandmarkDatabase::getObservations() const { return obs; } diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 52101f0..5c286e9 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -251,7 +251,7 @@ void NfrMapper::optimize(int num_iterations) { auto t1 = std::chrono::high_resolution_clock::now(); double rld_error; - Eigen::vector rld_vec; + Eigen::aligned_vector rld_vec; linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); // SparseHashAccumulator accum; @@ -269,12 +269,12 @@ void NfrMapper::optimize(int num_iterations) { MapperLinearizeAbsReduce> lopt(aom, &frame_poses); - tbb::blocked_range::iterator> range( + tbb::blocked_range::iterator> range( rld_vec.begin(), rld_vec.end()); - tbb::blocked_range::const_iterator> range1( - roll_pitch_factors.begin(), roll_pitch_factors.end()); - tbb::blocked_range::const_iterator> range2( - rel_pose_factors.begin(), rel_pose_factors.end()); + tbb::blocked_range::const_iterator> + range1(roll_pitch_factors.begin(), roll_pitch_factors.end()); + tbb::blocked_range::const_iterator> + range2(rel_pose_factors.begin(), rel_pose_factors.end()); tbb::parallel_reduce(range, lopt); @@ -420,7 +420,7 @@ void NfrMapper::optimize(int num_iterations) { } } -Eigen::map& NfrMapper::getFramePoses() { +Eigen::aligned_map& NfrMapper::getFramePoses() { return frame_poses; } diff --git a/src/vi_estimator/vio_estimator.cpp b/src/vi_estimator/vio_estimator.cpp index 916c713..b6b017d 100644 --- a/src/vi_estimator/vio_estimator.cpp +++ b/src/vi_estimator/vio_estimator.cpp @@ -55,11 +55,11 @@ VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator( } double alignSVD(const std::vector& filter_t_ns, - const Eigen::vector& filter_t_w_i, + const Eigen::aligned_vector& filter_t_w_i, const std::vector& gt_t_ns, - Eigen::vector& gt_t_w_i) { - Eigen::vector est_associations; - Eigen::vector gt_associations; + Eigen::aligned_vector& gt_t_w_i) { + Eigen::aligned_vector est_associations; + Eigen::aligned_vector gt_associations; for (size_t i = 0; i < filter_t_w_i.size(); i++) { int64_t t_ns = filter_t_ns[i]; diff --git a/src/vio.cpp b/src/vio.cpp index a4ed462..c16a608 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -121,11 +121,11 @@ tbb::concurrent_bounded_queue out_vis_queue; tbb::concurrent_bounded_queue out_state_queue; std::vector vio_t_ns; -Eigen::vector vio_t_w_i; -Eigen::vector vio_T_w_i; +Eigen::aligned_vector vio_t_w_i; +Eigen::aligned_vector vio_T_w_i; std::vector gt_t_ns; -Eigen::vector gt_t_w_i; +Eigen::aligned_vector gt_t_w_i; std::string marg_data_path; size_t last_frame_processed = 0; @@ -640,8 +640,8 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (it != vis_map.end()) { - const Eigen::map& kp_map = - it->second->opt_flow_res->observations[cam_id]; + const Eigen::aligned_map& + kp_map = it->second->opt_flow_res->observations[cam_id]; for (const auto& kv : kp_map) { Eigen::MatrixXf transformed_patch = @@ -678,8 +678,8 @@ void draw_scene(pangolin::View& view) { glColor3ubv(cam_color); if (!vio_t_w_i.empty()) { - Eigen::vector sub_gt(vio_t_w_i.begin(), - vio_t_w_i.begin() + show_frame); + Eigen::aligned_vector sub_gt( + vio_t_w_i.begin(), vio_t_w_i.begin() + show_frame); pangolin::glDrawLineStrip(sub_gt); } diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index 1d71859..bc07bd2 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -97,12 +97,12 @@ std::normal_distribution<> obs_noise_dist{0, obs_std_dev}; basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); -Eigen::vector gt_points; -Eigen::vector gt_frame_T_w_i; -Eigen::vector gt_frame_t_w_i, vio_t_w_i; +Eigen::aligned_vector gt_points; +Eigen::aligned_vector gt_frame_T_w_i; +Eigen::aligned_vector gt_frame_t_w_i, vio_t_w_i; std::vector gt_frame_t_ns; -Eigen::vector gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, - noisy_accel, noisy_gyro, gt_vel; +Eigen::aligned_vector gt_accel, gt_gyro, gt_accel_bias, + gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel; std::vector gt_imu_t_ns; std::map gt_observations; @@ -730,7 +730,7 @@ void gen_data() { int64_t t_ns = gt_spline.getDtNs(); - Eigen::vector knots; + Eigen::aligned_vector knots; for (size_t i = 0; i < gt_spline.numKnots(); i++) { knots.push_back(gt_spline.getKnot(i)); } diff --git a/test/src/test_nfr.cpp b/test/src/test_nfr.cpp index 7f28686..726a9c7 100644 --- a/test/src/test_nfr.cpp +++ b/test/src/test_nfr.cpp @@ -22,11 +22,11 @@ std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev}; std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; TEST(PreIntegrationTestSuite, RelPoseTest) { - Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); - Sophus::SE3d T_w_j = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_j = Sophus::se3_expd(Sophus::Vector6d::Random()); - Sophus::SE3d T_i_j = - Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i.inverse() * T_w_j; + Sophus::SE3d T_i_j = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) * + T_w_i.inverse() * T_w_j; Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j; basalt::relPoseError(T_i_j, T_w_i, T_w_j, &d_res_d_T_w_i, &d_res_d_T_w_j); @@ -61,7 +61,7 @@ TEST(PreIntegrationTestSuite, RelPoseTest) { } TEST(PreIntegrationTestSuite, AbsPositionTest) { - Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random()); Eigen::Vector3d pos = T_w_i.translation() + Eigen::Vector3d::Random() / 10; @@ -84,12 +84,12 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) { } TEST(PreIntegrationTestSuite, YawTest) { - Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random()); Eigen::Vector3d yaw_dir_body = T_w_i.so3().inverse() * Eigen::Vector3d::UnitX(); - T_w_i = Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i; + T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) * T_w_i; Sophus::Matrix d_res_d_T_w_i; basalt::yawError(T_w_i, yaw_dir_body, &d_res_d_T_w_i); @@ -112,11 +112,11 @@ TEST(PreIntegrationTestSuite, YawTest) { } TEST(PreIntegrationTestSuite, RollPitchTest) { - Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random()); Sophus::SO3d R_w_i = T_w_i.so3(); - T_w_i = Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i; + T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) * T_w_i; Sophus::Matrix d_res_d_T_w_i; basalt::rollPitchError(T_w_i, R_w_i, &d_res_d_T_w_i); diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp index 6ffa531..df1fe20 100644 --- a/test/src/test_vio.cpp +++ b/test/src/test_vio.cpp @@ -74,7 +74,7 @@ TEST(VioTestSuite, ImuNullspace2Test) { state1.t_ns = imu_meas.get_dt_ns(); state1.T_w_i = gt_spline.pose(imu_meas.get_dt_ns()) * - Sophus::expd(Sophus::Vector6d::Random() / 10); + Sophus::se3_expd(Sophus::Vector6d::Random() / 10); state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) + Sophus::Vector3d::Random() / 10; state1.bias_gyro = bg; @@ -86,9 +86,9 @@ TEST(VioTestSuite, ImuNullspace2Test) { Eigen::Vector3d accel_weight; accel_weight.setConstant(1e6); - Eigen::map imu_meas_vec; - Eigen::map frame_states; - Eigen::map frame_poses; + Eigen::aligned_map imu_meas_vec; + Eigen::aligned_map frame_states; + Eigen::aligned_map frame_poses; imu_meas_vec[state0.t_ns] = imu_meas; frame_states[state0.t_ns] = state0; @@ -227,7 +227,7 @@ TEST(VioTestSuite, ImuNullspace3Test) { state1.t_ns = imu_meas1.get_dt_ns(); state1.T_w_i = gt_spline.pose(state1.t_ns) * - Sophus::expd(Sophus::Vector6d::Random() / 10); + Sophus::se3_expd(Sophus::Vector6d::Random() / 10); state1.vel_w_i = gt_spline.transVelWorld(state1.t_ns) + Sophus::Vector3d::Random() / 10; state1.bias_gyro = bg; @@ -235,7 +235,7 @@ TEST(VioTestSuite, ImuNullspace3Test) { state2.t_ns = imu_meas1.get_dt_ns() + imu_meas2.get_dt_ns(); state2.T_w_i = gt_spline.pose(state2.t_ns) * - Sophus::expd(Sophus::Vector6d::Random() / 10); + Sophus::se3_expd(Sophus::Vector6d::Random() / 10); state2.vel_w_i = gt_spline.transVelWorld(state2.t_ns) + Sophus::Vector3d::Random() / 10; state2.bias_gyro = bg; @@ -247,9 +247,9 @@ TEST(VioTestSuite, ImuNullspace3Test) { Eigen::Vector3d accel_weight; accel_weight.setConstant(1e6); - Eigen::map imu_meas_vec; - Eigen::map frame_states; - Eigen::map frame_poses; + Eigen::aligned_map imu_meas_vec; + Eigen::aligned_map frame_states; + Eigen::aligned_map frame_poses; imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1; imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2; @@ -287,11 +287,11 @@ TEST(VioTestSuite, ImuNullspace3Test) { } TEST(VioTestSuite, RelPoseTest) { - Sophus::SE3d T_w_i_h = Sophus::expd(Sophus::Vector6d::Random()); - Sophus::SE3d T_w_i_t = Sophus::expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i_h = Sophus::se3_expd(Sophus::Vector6d::Random()); + Sophus::SE3d T_w_i_t = Sophus::se3_expd(Sophus::Vector6d::Random()); - Sophus::SE3d T_i_c_h = Sophus::expd(Sophus::Vector6d::Random() / 10); - Sophus::SE3d T_i_c_t = Sophus::expd(Sophus::Vector6d::Random() / 10); + Sophus::SE3d T_i_c_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 10); + Sophus::SE3d T_i_c_t = Sophus::se3_expd(Sophus::Vector6d::Random() / 10); Sophus::Matrix6d d_rel_d_h, d_rel_d_t; @@ -311,7 +311,7 @@ TEST(VioTestSuite, RelPoseTest) { basalt::KeypointVioEstimator::computeRelPose(T_w_h_new, T_i_c_h, T_w_i_t, T_i_c_t); - return Sophus::logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); + return Sophus::se3_logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); }, x0); } @@ -328,7 +328,7 @@ TEST(VioTestSuite, RelPoseTest) { Sophus::SE3d T_t_h_sophus_new = basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h, T_w_t_new, T_i_c_t); - return Sophus::logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); + return Sophus::se3_logd(T_t_h_sophus_new * T_t_h_sophus.inverse()); }, x0); } @@ -345,8 +345,8 @@ TEST(VioTestSuite, LinearizePointsTest) { kpt_pos.dir = basalt::StereographicParam::project(point3d); kpt_pos.id = 0.1231231; - Sophus::SE3d T_w_h = Sophus::expd(Sophus::Vector6d::Random() / 100); - Sophus::SE3d T_w_t = Sophus::expd(Sophus::Vector6d::Random() / 100); + Sophus::SE3d T_w_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 100); + Sophus::SE3d T_w_t = Sophus::se3_expd(Sophus::Vector6d::Random() / 100); T_w_t.translation()[0] += 0.1; Sophus::SE3d T_t_h_sophus = T_w_t.inverse() * T_w_h; @@ -374,7 +374,8 @@ TEST(VioTestSuite, LinearizePointsTest) { 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::Matrix4d T_t_h_new = + (Sophus::se3_expd(x) * T_t_h_sophus).matrix(); Eigen::Vector2d res; basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos, diff --git a/thirdparty/apriltag/include/basalt/utils/apriltag.h b/thirdparty/apriltag/include/basalt/utils/apriltag.h index 84b1536..612979b 100644 --- a/thirdparty/apriltag/include/basalt/utils/apriltag.h +++ b/thirdparty/apriltag/include/basalt/utils/apriltag.h @@ -14,9 +14,9 @@ class ApriltagDetector { ~ApriltagDetector(); void detectTags(basalt::ManagedImage& img_raw, - Eigen::vector& corners, + Eigen::aligned_vector& corners, std::vector& ids, std::vector& radii, - Eigen::vector& corners_rejected, + Eigen::aligned_vector& corners_rejected, std::vector& ids_rejected, std::vector& radii_rejected); diff --git a/thirdparty/apriltag/src/apriltag.cpp b/thirdparty/apriltag/src/apriltag.cpp index 391766d..1aaa533 100644 --- a/thirdparty/apriltag/src/apriltag.cpp +++ b/thirdparty/apriltag/src/apriltag.cpp @@ -39,9 +39,9 @@ ApriltagDetector::~ApriltagDetector() { delete data; } void ApriltagDetector::detectTags( basalt::ManagedImage& img_raw, - Eigen::vector& corners, std::vector& ids, + Eigen::aligned_vector& corners, std::vector& ids, std::vector& radii, - Eigen::vector& corners_rejected, + Eigen::aligned_vector& corners_rejected, std::vector& ids_rejected, std::vector& radii_rejected) { corners.clear(); ids.clear(); diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index ff561d2..7847c46 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit ff561d2369d8dd159ff1e7316e451bec41544ebe +Subproject commit 7847c4699607eaca370427ae4e2e840c5b502526