ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

varialble renaming

This commit is contained in:
Vladyslav Usenko 2019-10-04 15:38:04 +02:00
parent f71f252794
commit 8d2da587cf
55 changed files with 415 additions and 362 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/basalt/calibration/aprilgrid.h
View File

@ -41,8 +41,8 @@ namespace basalt {
struct AprilGrid { struct AprilGrid {
AprilGrid(const std::string &config_path); AprilGrid(const std::string &config_path);
Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d; Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
Eigen::vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d; Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
private: private:
int tagCols; // number of apriltags int tagCols; // number of apriltags

20
include/basalt/calibration/calibration_helper.h
View File

@ -44,7 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt { namespace basalt {
struct CalibCornerData { struct CalibCornerData {
Eigen::vector<Eigen::Vector2d> corners; Eigen::aligned_vector<Eigen::Vector2d> corners;
std::vector<int> corner_ids; std::vector<int> corner_ids;
std::vector<double> radii; //!< threshold used for maximum displacement std::vector<double> radii; //!< threshold used for maximum displacement
//! during sub-pix refinement; Search region is //! during sub-pix refinement; Search region is
@ -53,7 +53,7 @@ struct CalibCornerData {
}; };
struct ProjectedCornerData { struct ProjectedCornerData {
Eigen::vector<Eigen::Vector2d> corners_proj; Eigen::aligned_vector<Eigen::Vector2d> corners_proj;
std::vector<bool> corners_proj_success; std::vector<bool> corners_proj_success;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -63,7 +63,7 @@ struct CalibInitPoseData {
Sophus::SE3d T_a_c; Sophus::SE3d T_a_c;
size_t num_inliers; size_t num_inliers;
Eigen::vector<Eigen::Vector2d> reprojected_corners; Eigen::aligned_vector<Eigen::Vector2d> reprojected_corners;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
@ -78,16 +78,16 @@ class CalibHelper {
static void initCamPoses( static void initCamPoses(
const Calibration<double>::Ptr& calib, const Calibration<double>::Ptr& calib,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>& calib_corners, tbb::concurrent_unordered_map<TimeCamId, CalibCornerData>& calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData>& tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData>&
calib_init_poses); calib_init_poses);
static bool initializeIntrinsics( static bool initializeIntrinsics(
const Eigen::vector<Eigen::Vector2d>& corners, const Eigen::aligned_vector<Eigen::Vector2d>& corners,
const std::vector<int>& corner_ids, const std::vector<int>& corner_ids,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d, int cols, const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
int rows, Eigen::Vector4d& init_intr); int cols, int rows, Eigen::Vector4d& init_intr);
private: private:
inline static double square(double x) { return x * x; } inline static double square(double x) { return x * x; }
@ -98,14 +98,14 @@ class CalibHelper {
static void computeInitialPose( static void computeInitialPose(
const Calibration<double>::Ptr& calib, size_t cam_id, const Calibration<double>::Ptr& calib, size_t cam_id,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp); const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp);
static size_t computeReprojectionError( static size_t computeReprojectionError(
const UnifiedCamera<double>& cam_calib, const UnifiedCamera<double>& cam_calib,
const Eigen::vector<Eigen::Vector2d>& corners, const Eigen::aligned_vector<Eigen::Vector2d>& corners,
const std::vector<int>& corner_ids, const std::vector<int>& corner_ids,
const Eigen::vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
const Sophus::SE3d& T_target_camera, double& error); const Sophus::SE3d& T_target_camera, double& error);
}; };

20
include/basalt/calibration/vignette.h
View File

@ -47,12 +47,13 @@ class VignetteEstimator {
static const int64_t knot_spacing = 1e10; static const int64_t knot_spacing = 1e10;
static const int border_size = 2; static const int border_size = 2;
VignetteEstimator(const VioDatasetPtr &vio_dataset, VignetteEstimator(
const Eigen::vector<Eigen::Vector2d> &optical_centers, const VioDatasetPtr &vio_dataset,
const Eigen::vector<Eigen::Vector2i> &resolutions, const Eigen::aligned_vector<Eigen::Vector2d> &optical_centers,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
&reprojected_vignette, const std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
const AprilGrid &april_grid); &reprojected_vignette,
const AprilGrid &april_grid);
void compute_error(std::map<TimeCamId, std::vector<double>> void compute_error(std::map<TimeCamId, std::vector<double>>
*reprojected_vignette_error = nullptr); *reprojected_vignette_error = nullptr);
@ -75,9 +76,10 @@ class VignetteEstimator {
private: private:
const VioDatasetPtr vio_dataset; const VioDatasetPtr vio_dataset;
Eigen::vector<Eigen::Vector2d> optical_centers; Eigen::aligned_vector<Eigen::Vector2d> optical_centers;
Eigen::vector<Eigen::Vector2i> resolutions; Eigen::aligned_vector<Eigen::Vector2i> resolutions;
std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> reprojected_vignette; std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
reprojected_vignette;
const AprilGrid &april_grid; const AprilGrid &april_grid;
size_t vign_size; size_t vign_size;

2
include/basalt/device/rs_t265.h
View File

@ -105,7 +105,7 @@ class RsT265Device {
int frame_counter = 0; int frame_counter = 0;
Eigen::deque<RsIMUData> gyro_data_queue; Eigen::aligned_deque<RsIMUData> gyro_data_queue;
std::shared_ptr<RsIMUData> prev_accel_data; std::shared_ptr<RsIMUData> prev_accel_data;
std::shared_ptr<basalt::Calibration<double>> calib; std::shared_ptr<basalt::Calibration<double>> calib;

11
include/basalt/io/dataset_io.h
View File

@ -79,7 +79,7 @@ struct ImageData {
}; };
struct Observations { struct Observations {
Eigen::vector<Eigen::Vector2d> pos; Eigen::aligned_vector<Eigen::Vector2d> pos;
std::vector<int> id; std::vector<int> id;
}; };
@ -111,7 +111,7 @@ struct AprilgridCornersData {
int64_t timestamp_ns; int64_t timestamp_ns;
int cam_id; int cam_id;
Eigen::vector<Eigen::Vector2d> corner_pos; Eigen::aligned_vector<Eigen::Vector2d> corner_pos;
std::vector<int> corner_id; std::vector<int> corner_id;
}; };
@ -123,10 +123,11 @@ class VioDataset {
virtual std::vector<int64_t> &get_image_timestamps() = 0; virtual std::vector<int64_t> &get_image_timestamps() = 0;
virtual const Eigen::vector<AccelData> &get_accel_data() const = 0; virtual const Eigen::aligned_vector<AccelData> &get_accel_data() const = 0;
virtual const Eigen::vector<GyroData> &get_gyro_data() const = 0; virtual const Eigen::aligned_vector<GyroData> &get_gyro_data() const = 0;
virtual const std::vector<int64_t> &get_gt_timestamps() const = 0; virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
virtual const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const = 0; virtual const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data()
const = 0;
virtual int64_t get_mocap_to_imu_offset_ns() const = 0; virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0; virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;

19
include/basalt/io/dataset_io_euroc.h
View File

@ -57,11 +57,12 @@ class EurocVioDataset : public VioDataset {
// missing frames // missing frames
// std::unordered_map<int64_t, std::vector<ImageData>> image_data; // std::unordered_map<int64_t, std::vector<ImageData>> image_data;
Eigen::vector<AccelData> accel_data; Eigen::aligned_vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data; Eigen::aligned_vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned Eigen::aligned_vector<Sophus::SE3d>
gt_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns = 0; int64_t mocap_to_imu_offset_ns = 0;
@ -74,12 +75,16 @@ class EurocVioDataset : public VioDataset {
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; } std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; } const Eigen::aligned_vector<AccelData> &get_accel_data() const {
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; } return accel_data;
}
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
return gyro_data;
}
const std::vector<int64_t> &get_gt_timestamps() const { const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps; return gt_timestamps;
} }
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const { const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data; return gt_pose_data;
} }

19
include/basalt/io/dataset_io_kitti.h
View File

@ -57,11 +57,12 @@ class KittiVioDataset : public VioDataset {
// missing frames // missing frames
// std::unordered_map<int64_t, std::vector<ImageData>> image_data; // std::unordered_map<int64_t, std::vector<ImageData>> image_data;
Eigen::vector<AccelData> accel_data; Eigen::aligned_vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data; Eigen::aligned_vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned Eigen::aligned_vector<Sophus::SE3d>
gt_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns; int64_t mocap_to_imu_offset_ns;
@ -72,12 +73,16 @@ class KittiVioDataset : public VioDataset {
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; } std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; } const Eigen::aligned_vector<AccelData> &get_accel_data() const {
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; } return accel_data;
}
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
return gyro_data;
}
const std::vector<int64_t> &get_gt_timestamps() const { const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps; return gt_timestamps;
} }
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const { const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data; return gt_pose_data;
} }
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }

19
include/basalt/io/dataset_io_rosbag.h
View File

@ -71,11 +71,12 @@ class RosbagVioDataset : public VioDataset {
std::unordered_map<int64_t, std::vector<std::optional<rosbag::IndexEntry>>> std::unordered_map<int64_t, std::vector<std::optional<rosbag::IndexEntry>>>
image_data_idx; image_data_idx;
Eigen::vector<AccelData> accel_data; Eigen::aligned_vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data; Eigen::aligned_vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned Eigen::aligned_vector<Sophus::SE3d>
gt_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns; int64_t mocap_to_imu_offset_ns;
@ -86,12 +87,16 @@ class RosbagVioDataset : public VioDataset {
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; } std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; } const Eigen::aligned_vector<AccelData> &get_accel_data() const {
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; } return accel_data;
}
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
return gyro_data;
}
const std::vector<int64_t> &get_gt_timestamps() const { const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps; return gt_timestamps;
} }
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const { const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data; return gt_pose_data;
} }

19
include/basalt/io/dataset_io_uzh.h
View File

@ -58,11 +58,12 @@ class UzhVioDataset : public VioDataset {
// missing frames // missing frames
// std::unordered_map<int64_t, std::vector<ImageData>> image_data; // std::unordered_map<int64_t, std::vector<ImageData>> image_data;
Eigen::vector<AccelData> accel_data; Eigen::aligned_vector<AccelData> accel_data;
Eigen::vector<GyroData> gyro_data; Eigen::aligned_vector<GyroData> gyro_data;
std::vector<int64_t> gt_timestamps; // ordered gt timestamps std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned Eigen::aligned_vector<Sophus::SE3d>
gt_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns = 0; int64_t mocap_to_imu_offset_ns = 0;
@ -75,12 +76,16 @@ class UzhVioDataset : public VioDataset {
std::vector<int64_t> &get_image_timestamps() { return image_timestamps; } std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; } const Eigen::aligned_vector<AccelData> &get_accel_data() const {
const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; } return accel_data;
}
const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
return gyro_data;
}
const std::vector<int64_t> &get_gt_timestamps() const { const std::vector<int64_t> &get_gt_timestamps() const {
return gt_timestamps; return gt_timestamps;
} }
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const { const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data; return gt_pose_data;
} }

22
include/basalt/optical_flow/frame_to_frame_optical_flow.h
View File

@ -176,15 +176,16 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
frame_counter++; frame_counter++;
} }
void trackPoints( void trackPoints(const basalt::ManagedImagePyr<u_int16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_1, const basalt::ManagedImagePyr<u_int16_t>& pyr_2,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2, const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
const Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_1, transform_map_1,
Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_2) const { Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_2) const {
size_t num_points = transform_map_1.size(); size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids; std::vector<KeypointId> ids;
Eigen::vector<Eigen::AffineCompact2f> init_vec; Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
ids.reserve(num_points); ids.reserve(num_points);
init_vec.reserve(num_points); init_vec.reserve(num_points);
@ -292,7 +293,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
} }
void addPoints() { void addPoints() {
Eigen::vector<Eigen::Vector2d> pts0; Eigen::aligned_vector<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) { for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>()); pts0.emplace_back(kv.second.translation().cast<double>());
@ -303,7 +304,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
detectKeypoints(pyramid->at(0).lvl(0), kd, detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0); config.optical_flow_detection_grid_size, 1, pts0);
Eigen::map<KeypointId, Eigen::AffineCompact2f> new_poses0, new_poses1; Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
new_poses1;
for (size_t i = 0; i < kd.corners.size(); i++) { for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::AffineCompact2f transform; Eigen::AffineCompact2f transform;
@ -331,7 +333,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
std::set<KeypointId> lm_to_remove; std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid; std::vector<KeypointId> kpid;
Eigen::vector<Eigen::Vector2f> proj0, proj1; Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) { for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first); auto it = transforms->observations.at(0).find(kv.first);
@ -343,7 +345,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
} }
} }
Eigen::vector<Eigen::Vector4f> p3d0, p3d1; Eigen::aligned_vector<Eigen::Vector4f> p3d0, p3d1;
std::vector<bool> p3d0_success, p3d1_success; std::vector<bool> p3d0_success, p3d1_success;
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);

3
include/basalt/optical_flow/optical_flow.h
View File

@ -62,7 +62,8 @@ struct OpticalFlowResult {
using Ptr = std::shared_ptr<OpticalFlowResult>; using Ptr = std::shared_ptr<OpticalFlowResult>;
int64_t t_ns; int64_t t_ns;
std::vector<Eigen::map<KeypointId, Eigen::AffineCompact2f>> observations; std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
observations;
OpticalFlowInput::Ptr input_images; OpticalFlowInput::Ptr input_images;
}; };

31
include/basalt/optical_flow/patch_optical_flow.h
View File

@ -164,15 +164,16 @@ class PatchOpticalFlow : public OpticalFlowBase {
frame_counter++; frame_counter++;
} }
void trackPoints( void trackPoints(const basalt::ManagedImagePyr<u_int16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_1, const basalt::ManagedImagePyr<u_int16_t>& pyr_2,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2, const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
const Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_1, transform_map_1,
Eigen::map<KeypointId, Eigen::AffineCompact2f>& transform_map_2) const { Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_2) const {
size_t num_points = transform_map_1.size(); size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids; std::vector<KeypointId> ids;
Eigen::vector<Eigen::AffineCompact2f> init_vec; Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
ids.reserve(num_points); ids.reserve(num_points);
init_vec.reserve(num_points); init_vec.reserve(num_points);
@ -191,7 +192,7 @@ class PatchOpticalFlow : public OpticalFlowBase {
const Eigen::AffineCompact2f& transform_1 = init_vec[r]; const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1; Eigen::AffineCompact2f transform_2 = transform_1;
const Eigen::vector<PatchT>& patch_vec = patches.at(id); const Eigen::aligned_vector<PatchT>& patch_vec = patches.at(id);
bool valid = trackPoint(pyr_2, patch_vec, transform_2); bool valid = trackPoint(pyr_2, patch_vec, transform_2);
@ -223,7 +224,7 @@ class PatchOpticalFlow : public OpticalFlowBase {
} }
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& pyr, inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& pyr,
const Eigen::vector<PatchT>& patch_vec, const Eigen::aligned_vector<PatchT>& patch_vec,
Eigen::AffineCompact2f& transform) const { Eigen::AffineCompact2f& transform) const {
bool patch_valid = true; bool patch_valid = true;
@ -276,7 +277,7 @@ class PatchOpticalFlow : public OpticalFlowBase {
} }
void addPoints() { void addPoints() {
Eigen::vector<Eigen::Vector2d> pts0; Eigen::aligned_vector<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) { for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>()); pts0.emplace_back(kv.second.translation().cast<double>());
@ -287,10 +288,11 @@ class PatchOpticalFlow : public OpticalFlowBase {
detectKeypoints(pyramid->at(0).lvl(0), kd, detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0); config.optical_flow_detection_grid_size, 1, pts0);
Eigen::map<KeypointId, Eigen::AffineCompact2f> new_poses0, new_poses1; Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
new_poses1;
for (size_t i = 0; i < kd.corners.size(); i++) { for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::vector<PatchT>& p = patches[last_keypoint_id]; Eigen::aligned_vector<PatchT>& p = patches[last_keypoint_id];
Vector2 pos = kd.corners[i].cast<Scalar>(); Vector2 pos = kd.corners[i].cast<Scalar>();
@ -325,7 +327,7 @@ class PatchOpticalFlow : public OpticalFlowBase {
std::set<KeypointId> lm_to_remove; std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid; std::vector<KeypointId> kpid;
Eigen::vector<Eigen::Vector2d> proj0, proj1; Eigen::aligned_vector<Eigen::Vector2d> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) { for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first); auto it = transforms->observations.at(0).find(kv.first);
@ -337,7 +339,7 @@ class PatchOpticalFlow : public OpticalFlowBase {
} }
} }
Eigen::vector<Eigen::Vector4d> p3d0, p3d1; Eigen::aligned_vector<Eigen::Vector4d> p3d0, p3d1;
std::vector<bool> p3d0_success, p3d1_success; std::vector<bool> p3d0_success, p3d1_success;
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);
@ -372,7 +374,8 @@ class PatchOpticalFlow : public OpticalFlowBase {
VioConfig config; VioConfig config;
basalt::Calibration<double> calib; basalt::Calibration<double> calib;
Eigen::unordered_map<KeypointId, Eigen::vector<PatchT>> patches; Eigen::aligned_unordered_map<KeypointId, Eigen::aligned_vector<PatchT>>
patches;
OpticalFlowResult::Ptr transforms; OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid, std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid,

12
include/basalt/optimization/linearize.h
View File

@ -72,10 +72,11 @@ struct LinearizeBase {
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<PoseData>::const_iterator PoseDataIter; typedef typename Eigen::aligned_vector<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::vector<GyroData>::const_iterator GyroDataIter; typedef typename Eigen::aligned_vector<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::vector<AccelData>::const_iterator AccelDataIter; typedef
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator typename Eigen::aligned_vector<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter; AprilgridCornersDataIter;
template <int INTRINSICS_SIZE> template <int INTRINSICS_SIZE>
@ -100,7 +101,8 @@ struct LinearizeBase {
struct CalibCommonData { struct CalibCommonData {
const Calibration<Scalar>* calibration = nullptr; const Calibration<Scalar>* calibration = nullptr;
const MocapCalibration<Scalar>* mocap_calibration = nullptr; const MocapCalibration<Scalar>* mocap_calibration = nullptr;
const Eigen::vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d = nullptr; const Eigen::aligned_vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d =
nullptr;
// Calib data // Calib data
const std::vector<size_t>* offset_T_i_c = nullptr; const std::vector<size_t>* offset_T_i_c = nullptr;

17
include/basalt/optimization/poses_linearize.h
View File

@ -63,7 +63,7 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter; AprilgridCornersDataIter;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData; typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
@ -75,11 +75,12 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
size_t opt_size; size_t opt_size;
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose; const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
LinearizePosesOpt(size_t opt_size, LinearizePosesOpt(
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose, size_t opt_size,
const CalibCommonData& common_data) const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
const CalibCommonData& common_data)
: opt_size(opt_size), timestam_to_pose(timestam_to_pose) { : opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
this->common_data = common_data; this->common_data = common_data;
accum.reset(opt_size); accum.reset(opt_size);
@ -191,7 +192,7 @@ struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter; AprilgridCornersDataIter;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData; typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
@ -202,11 +203,11 @@ struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
size_t opt_size; size_t opt_size;
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose; const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose;
ComputeErrorPosesOpt( ComputeErrorPosesOpt(
size_t opt_size, size_t opt_size,
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose, const Eigen::aligned_unordered_map<int64_t, SE3>& timestam_to_pose,
const CalibCommonData& common_data) const CalibCommonData& common_data)
: opt_size(opt_size), timestam_to_pose(timestam_to_pose) { : opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
this->common_data = common_data; this->common_data = common_data;

28
include/basalt/optimization/poses_optimize.h
View File

@ -55,17 +55,17 @@ class PosesOptimization {
using VectorX = typename LinearizeT::VectorX; using VectorX = typename LinearizeT::VectorX;
using AprilgridCornersDataIter = using AprilgridCornersDataIter =
typename Eigen::vector<AprilgridCornersData>::const_iterator; typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator;
public: public:
PosesOptimization() PosesOptimization()
: lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {} : lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
bool initializeIntrinsics( bool initializeIntrinsics(
size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners, size_t cam_id, const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids, const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, int cols, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
int rows) { int cols, int rows) {
Eigen::Vector4d init_intr; Eigen::Vector4d init_intr;
bool val = CalibHelper::initializeIntrinsics( bool val = CalibHelper::initializeIntrinsics(
@ -156,8 +156,8 @@ class PosesOptimization {
int max_iter = 10; int max_iter = 10;
while (!step && max_iter > 0 && !converged) { while (!step && max_iter > 0 && !converged) {
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup = Eigen::aligned_unordered_map<int64_t, Sophus::SE3d>
timestam_to_pose; timestam_to_pose_backup = timestam_to_pose;
Calibration<Scalar> calib_backup = *calib; Calibration<Scalar> calib_backup = *calib;
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
@ -170,12 +170,12 @@ class PosesOptimization {
for (auto &kv : timestam_to_pose) { for (auto &kv : timestam_to_pose) {
kv.second *= kv.second *=
Sophus::expd(inc.segment<POSE_SIZE>(offset_poses[kv.first])); Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
} }
for (size_t i = 0; i < calib->T_i_c.size(); i++) { for (size_t i = 0; i < calib->T_i_c.size(); i++) {
calib->T_i_c[i] *= calib->T_i_c[i] *=
Sophus::expd(inc.segment<POSE_SIZE>(offset_T_i_c[i])); Sophus::se3_expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
} }
for (size_t i = 0; i < calib->intrinsics.size(); i++) { for (size_t i = 0; i < calib->intrinsics.size(); i++) {
@ -264,7 +264,7 @@ class PosesOptimization {
return it->second; return it->second;
} }
void setAprilgridCorners3d(const Eigen::vector<Eigen::Vector4d> &v) { void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d> &v) {
aprilgrid_corner_pos_3d = v; aprilgrid_corner_pos_3d = v;
} }
@ -280,7 +280,7 @@ class PosesOptimization {
void addAprilgridMeasurement( void addAprilgridMeasurement(
int64_t t_ns, int cam_id, int64_t t_ns, int cam_id,
const Eigen::vector<Eigen::Vector2d> &corners_pos, const Eigen::aligned_vector<Eigen::Vector2d> &corners_pos,
const std::vector<int> &corner_id) { const std::vector<int> &corner_id) {
aprilgrid_corners_measurements.emplace_back(); aprilgrid_corners_measurements.emplace_back();
@ -298,7 +298,7 @@ class PosesOptimization {
calib->vignette = vign; calib->vignette = vign;
} }
void setResolution(const Eigen::vector<Eigen::Vector2i> &resolution) { void setResolution(const Eigen::aligned_vector<Eigen::Vector2i> &resolution) {
calib->resolution = resolution; calib->resolution = resolution;
} }
@ -319,11 +319,11 @@ class PosesOptimization {
std::vector<size_t> offset_T_i_c; std::vector<size_t> offset_T_i_c;
// frame poses // frame poses
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose; Eigen::aligned_unordered_map<int64_t, Sophus::SE3d> timestam_to_pose;
Eigen::vector<AprilgridCornersData> aprilgrid_corners_measurements; Eigen::aligned_vector<AprilgridCornersData> aprilgrid_corners_measurements;
Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d; Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
}; // namespace basalt }; // namespace basalt

30
include/basalt/optimization/spline_linearize.h
View File

@ -80,13 +80,14 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
typedef Se3Spline<N, Scalar> SplineT; typedef Se3Spline<N, Scalar> SplineT;
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter; typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter; typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typedef
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter; typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
MocapPoseDataIter;
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH; // typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData; typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
@ -519,7 +520,7 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
const SE3 T_mark_moc_meas = pm.data.inverse(); 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 // TODO: check derivatives
Matrix6 d_res_d_T_i_mark; Matrix6 d_res_d_T_i_mark;
@ -659,13 +660,14 @@ struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
typedef Se3Spline<N, Scalar> SplineT; typedef Se3Spline<N, Scalar> SplineT;
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter; typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter; typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typedef
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter; typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
MocapPoseDataIter;
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH; // typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData; typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
@ -822,7 +824,7 @@ struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
const SE3 T_mark_moc_meas = pm.data.inverse(); 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; const Scalar& mocap_var_inv = this->common_data.mocap_var_inv;

37
include/basalt/optimization/spline_optimize.h
View File

@ -231,7 +231,7 @@ class SplineOptimization {
SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); } SE3 getT_w_i(int64_t t_ns) { return spline.pose(t_ns); }
void setAprilgridCorners3d(const Eigen::vector<Eigen::Vector4d>& v) { void setAprilgridCorners3d(const Eigen::aligned_vector<Eigen::Vector4d>& v) {
aprilgrid_corner_pos_3d = v; aprilgrid_corner_pos_3d = v;
} }
@ -270,7 +270,7 @@ class SplineOptimization {
void addAprilgridMeasurement( void addAprilgridMeasurement(
int64_t t_ns, int cam_id, int64_t t_ns, int cam_id,
const Eigen::vector<Eigen::Vector2d>& corners_pos, const Eigen::aligned_vector<Eigen::Vector2d>& corners_pos,
const std::vector<int>& corner_id) { const std::vector<int>& corner_id) {
min_time_us = std::min(min_time_us, t_ns); min_time_us = std::min(min_time_us, t_ns);
max_time_us = std::max(max_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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter; typedef typename Eigen::aligned_deque<PoseData>::const_iterator PoseDataIter;
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter; typedef typename Eigen::aligned_deque<GyroData>::const_iterator GyroDataIter;
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typedef
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter; typename Eigen::aligned_deque<AccelData>::const_iterator AccelDataIter;
typedef typename Eigen::aligned_deque<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typename Eigen::aligned_deque<MocapPoseData>::const_iterator
MocapPoseDataIter;
void applyInc(VectorX& inc_full, void applyInc(VectorX& inc_full,
const std::vector<size_t>& offset_cam_intrinsics) { const std::vector<size_t>& offset_cam_intrinsics) {
@ -550,7 +551,7 @@ class SplineOptimization {
size_t T_i_c_block_offset = size_t T_i_c_block_offset =
bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE; bias_block_offset + ACCEL_BIAS_SIZE + GYRO_BIAS_SIZE + G_SIZE;
for (size_t i = 0; i < calib->T_i_c.size(); i++) { for (size_t i = 0; i < calib->T_i_c.size(); i++) {
calib->T_i_c[i] *= Sophus::expd(inc_full.template segment<POSE_SIZE>( calib->T_i_c[i] *= Sophus::se3_expd(inc_full.template segment<POSE_SIZE>(
T_i_c_block_offset + i * POSE_SIZE)); T_i_c_block_offset + i * POSE_SIZE));
} }
@ -561,9 +562,9 @@ class SplineOptimization {
size_t mocap_block_offset = offset_cam_intrinsics.back(); size_t mocap_block_offset = offset_cam_intrinsics.back();
mocap_calib->T_moc_w *= mocap_calib->T_moc_w *= Sophus::se3_expd(
Sophus::expd(inc_full.template segment<POSE_SIZE>(mocap_block_offset)); inc_full.template segment<POSE_SIZE>(mocap_block_offset));
mocap_calib->T_i_mark *= Sophus::expd( mocap_calib->T_i_mark *= Sophus::se3_expd(
inc_full.template segment<POSE_SIZE>(mocap_block_offset + POSE_SIZE)); inc_full.template segment<POSE_SIZE>(mocap_block_offset + POSE_SIZE));
mocap_calib->mocap_time_offset_ns += mocap_calib->mocap_time_offset_ns +=
@ -580,11 +581,11 @@ class SplineOptimization {
int64_t min_time_us, max_time_us; int64_t min_time_us, max_time_us;
Eigen::deque<PoseData> pose_measurements; Eigen::aligned_deque<PoseData> pose_measurements;
Eigen::deque<GyroData> gyro_measurements; Eigen::aligned_deque<GyroData> gyro_measurements;
Eigen::deque<AccelData> accel_measurements; Eigen::aligned_deque<AccelData> accel_measurements;
Eigen::deque<AprilgridCornersData> aprilgrid_corners_measurements; Eigen::aligned_deque<AprilgridCornersData> aprilgrid_corners_measurements;
Eigen::deque<MocapPoseData> mocap_measurements; Eigen::aligned_deque<MocapPoseData> mocap_measurements;
typename LinearizeT::CalibCommonData ccd; typename LinearizeT::CalibCommonData ccd;
@ -597,7 +598,7 @@ class SplineOptimization {
SplineT spline; SplineT spline;
Vector3 g; Vector3 g;
Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d; Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
int64_t dt_ns; int64_t dt_ns;
}; // namespace basalt }; // namespace basalt

2
include/basalt/utils/common_types.h
View File

@ -102,7 +102,7 @@ struct KeypointsData {
/// FeatureId) /// FeatureId)
std::vector<std::bitset<256>> corner_descriptors; std::vector<std::bitset<256>> corner_descriptors;
Eigen::vector<Eigen::Vector4d> corners_3d; Eigen::aligned_vector<Eigen::Vector4d> corners_3d;
std::vector<FeatureHash> hashes; std::vector<FeatureHash> hashes;
HashBowVector bow_vector; HashBowVector bow_vector;

4
include/basalt/utils/imu_types.h
View File

@ -270,8 +270,8 @@ struct MargData {
AbsOrderMap aom; AbsOrderMap aom;
Eigen::MatrixXd abs_H; Eigen::MatrixXd abs_H;
Eigen::VectorXd abs_b; Eigen::VectorXd abs_b;
Eigen::map<int64_t, PoseVelBiasStateWithLin> frame_states; Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, PoseStateWithLin> frame_poses; Eigen::aligned_map<int64_t, PoseStateWithLin> frame_poses;
std::set<int64_t> kfs_all; std::set<int64_t> kfs_all;
std::set<int64_t> kfs_to_marg; std::set<int64_t> kfs_to_marg;
bool use_imu; bool use_imu;

10
include/basalt/utils/keypoints.h
View File

@ -53,11 +53,11 @@ 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(
KeypointsData& kd, int PATCH_SIZE = 32, const basalt::Image<const uint16_t>& img_raw, KeypointsData& kd,
int num_points_cell = 1, int PATCH_SIZE = 32, int num_points_cell = 1,
const Eigen::vector<Eigen::Vector2d>& current_points = const Eigen::aligned_vector<Eigen::Vector2d>& current_points =
Eigen::vector<Eigen::Vector2d>()); Eigen::aligned_vector<Eigen::Vector2d>());
void computeAngles(const basalt::Image<const uint16_t>& img_raw, void computeAngles(const basalt::Image<const uint16_t>& img_raw,
KeypointsData& kd, bool rotate_features); KeypointsData& kd, bool rotate_features);

2
include/basalt/utils/nfr.h
View File

@ -45,7 +45,7 @@ inline Sophus::Vector6d relPoseError(
const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr, 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::Matrix6d* d_res_d_T_w_j = nullptr) {
Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i; 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) { if (d_res_d_T_w_i || d_res_d_T_w_j) {
Sophus::Matrix6d J; Sophus::Matrix6d J;

2
include/basalt/utils/sim_utils.h
View File

@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace basalt { namespace basalt {
struct SimObservations { struct SimObservations {
Eigen::vector<Eigen::Vector2d> pos; Eigen::aligned_vector<Eigen::Vector2d> pos;
std::vector<int> id; std::vector<int> id;
}; };

2
include/basalt/utils/vis_utils.h
View File

@ -50,7 +50,7 @@ inline void render_camera(const Eigen::Matrix4d& T_w_c, float lineWidth,
const float sz = sizeFactor; const float sz = sizeFactor;
const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240; const float width = 640, height = 480, fx = 500, fy = 500, cx = 320, cy = 240;
const Eigen::vector<Eigen::Vector3f> lines = { const Eigen::aligned_vector<Eigen::Vector3f> lines = {
{0, 0, 0}, {0, 0, 0},
{sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz}, {sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz},
{0, 0, 0}, {0, 0, 0},

35
include/basalt/vi_estimator/ba_base.h
View File

@ -45,8 +45,8 @@ class BundleAdjustmentBase {
struct RelLinDataBase { struct RelLinDataBase {
std::vector<std::pair<TimeCamId, TimeCamId>> order; std::vector<std::pair<TimeCamId, TimeCamId>> order;
Eigen::vector<Sophus::Matrix6d> d_rel_d_h; Eigen::aligned_vector<Sophus::Matrix6d> d_rel_d_h;
Eigen::vector<Sophus::Matrix6d> d_rel_d_t; Eigen::aligned_vector<Sophus::Matrix6d> d_rel_d_t;
}; };
struct FrameRelLinData { struct FrameRelLinData {
@ -54,7 +54,7 @@ class BundleAdjustmentBase {
Sophus::Vector6d bp; Sophus::Vector6d bp;
std::vector<int> lm_id; std::vector<int> lm_id;
Eigen::vector<Eigen::Matrix<double, 6, 3>> Hpl; Eigen::aligned_vector<Eigen::Matrix<double, 6, 3>> Hpl;
FrameRelLinData() { FrameRelLinData() {
Hpp.setZero(); Hpp.setZero();
@ -88,11 +88,12 @@ class BundleAdjustmentBase {
} }
} }
Eigen::unordered_map<int, Eigen::Matrix3d> Hll; Eigen::aligned_unordered_map<int, Eigen::Matrix3d> Hll;
Eigen::unordered_map<int, Eigen::Vector3d> bl; Eigen::aligned_unordered_map<int, Eigen::Vector3d> bl;
Eigen::unordered_map<int, std::vector<std::pair<size_t, size_t>>> lm_to_obs; Eigen::aligned_unordered_map<int, std::vector<std::pair<size_t, size_t>>>
lm_to_obs;
Eigen::vector<FrameRelLinData> Hpppl; Eigen::aligned_vector<FrameRelLinData> Hpppl;
double error; double error;
}; };
@ -103,9 +104,11 @@ class BundleAdjustmentBase {
double outlier_threshold = 0) const; double outlier_threshold = 0) const;
void linearizeHelper( void linearizeHelper(
Eigen::vector<RelLinData>& rld_vec, Eigen::aligned_vector<RelLinData>& rld_vec,
const Eigen::map< const Eigen::aligned_map<
TimeCamId, Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>& TimeCamId,
Eigen::aligned_map<TimeCamId,
Eigen::aligned_vector<KeypointObservation>>>&
obs_to_lin, obs_to_lin,
double& error) const; double& error) const;
@ -227,7 +230,7 @@ class BundleAdjustmentBase {
Sophus::Matrix6d* d_rel_d_h = nullptr, Sophus::Matrix6d* d_rel_d_h = nullptr,
Sophus::Matrix6d* d_rel_d_t = nullptr); Sophus::Matrix6d* d_rel_d_t = nullptr);
void get_current_points(Eigen::vector<Eigen::Vector3d>& points, void get_current_points(Eigen::aligned_vector<Eigen::Vector3d>& points,
std::vector<int>& ids) const; std::vector<int>& ids) const;
// Modifies abs_H and abs_b as a side effect. // Modifies abs_H and abs_b as a side effect.
@ -254,8 +257,8 @@ class BundleAdjustmentBase {
static Eigen::VectorXd checkNullspace( static Eigen::VectorXd checkNullspace(
const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b, const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b,
const AbsOrderMap& marg_order, const AbsOrderMap& marg_order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses); const Eigen::aligned_map<int64_t, PoseStateWithLin>& frame_poses);
/// Triangulates the point and returns homogenous representation. First 3 /// Triangulates the point and returns homogenous representation. First 3
/// components - unit-length direction vector. Last component inverse /// components - unit-length direction vector. Last component inverse
@ -359,7 +362,7 @@ class BundleAdjustmentBase {
template <class AccumT> template <class AccumT>
struct LinearizeAbsReduce { struct LinearizeAbsReduce {
using RelLinDataIter = Eigen::vector<RelLinData>::iterator; using RelLinDataIter = Eigen::aligned_vector<RelLinData>::iterator;
LinearizeAbsReduce(AbsOrderMap& aom) : aom(aom) { LinearizeAbsReduce(AbsOrderMap& aom) : aom(aom) {
accum.reset(aom.total_size); accum.reset(aom.total_size);
@ -414,8 +417,8 @@ class BundleAdjustmentBase {
return PoseStateWithLin(it2->second); return PoseStateWithLin(it2->second);
} }
Eigen::map<int64_t, PoseVelBiasStateWithLin> frame_states; Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, PoseStateWithLin> frame_poses; Eigen::aligned_map<int64_t, PoseStateWithLin> frame_poses;
// Point management // Point management
LandmarkDatabase lmdb; LandmarkDatabase lmdb;

26
include/basalt/vi_estimator/keypoint_vio.h
View File

@ -84,16 +84,16 @@ class KeypointVioEstimator : public VioEstimatorBase,
static void linearizeAbsIMU( static void linearizeAbsIMU(
const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
double& imu_error, double& bg_error, double& ba_error, double& imu_error, double& bg_error, double& ba_error,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas, const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& gyro_bias_weight,
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g);
static void computeImuError( static void computeImuError(
const AbsOrderMap& aom, double& imu_error, double& bg_error, const AbsOrderMap& aom, double& imu_error, double& bg_error,
double& ba_error, double& ba_error,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas, const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& gyro_bias_weight,
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); 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<N, N>(); } // const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
void computeProjections( void computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& res) const; std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& res) const;
inline void setMaxStates(size_t val) { max_states = val; } inline void setMaxStates(size_t val) { max_states = val; }
inline void setMaxKfs(size_t val) { max_kfs = val; } inline void setMaxKfs(size_t val) { max_kfs = val; }
Eigen::vector<Sophus::SE3d> getFrameStates() const { Eigen::aligned_vector<Sophus::SE3d> getFrameStates() const {
Eigen::vector<Sophus::SE3d> res; Eigen::aligned_vector<Sophus::SE3d> res;
for (const auto& kv : frame_states) { for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i); res.push_back(kv.second.getState().T_w_i);
@ -153,8 +153,8 @@ class KeypointVioEstimator : public VioEstimatorBase,
return res; return res;
} }
Eigen::vector<Sophus::SE3d> getFramePoses() const { Eigen::aligned_vector<Sophus::SE3d> getFramePoses() const {
Eigen::vector<Sophus::SE3d> res; Eigen::aligned_vector<Sophus::SE3d> res;
for (const auto& kv : frame_poses) { for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose()); res.push_back(kv.second.getPose());
@ -163,8 +163,8 @@ class KeypointVioEstimator : public VioEstimatorBase,
return res; return res;
} }
Eigen::map<int64_t, Sophus::SE3d> getAllPosesMap() const { Eigen::aligned_map<int64_t, Sophus::SE3d> getAllPosesMap() const {
Eigen::map<int64_t, Sophus::SE3d> res; Eigen::aligned_map<int64_t, Sophus::SE3d> res;
for (const auto& kv : frame_poses) { for (const auto& kv : frame_poses) {
res[kv.first] = kv.second.getPose(); res[kv.first] = kv.second.getPose();
@ -187,13 +187,13 @@ class KeypointVioEstimator : public VioEstimatorBase,
std::set<int64_t> kf_ids; std::set<int64_t> kf_ids;
int64_t last_state_t_ns; int64_t last_state_t_ns;
Eigen::map<int64_t, IntegratedImuMeasurement> imu_meas; Eigen::aligned_map<int64_t, IntegratedImuMeasurement> imu_meas;
const Eigen::Vector3d g; const Eigen::Vector3d g;
// Input // Input
Eigen::map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res; Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> num_points_kf; std::map<int64_t, int> num_points_kf;

16
include/basalt/vi_estimator/keypoint_vo.h
View File

@ -120,13 +120,13 @@ class KeypointVoEstimator : public VioEstimatorBase,
// const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); } // const MatNN get_cov() const { return cov.bottomRightCorner<N, N>(); }
void computeProjections( void computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& res) const; std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& res) const;
inline void setMaxStates(size_t val) { max_states = val; } inline void setMaxStates(size_t val) { max_states = val; }
inline void setMaxKfs(size_t val) { max_kfs = val; } inline void setMaxKfs(size_t val) { max_kfs = val; }
Eigen::vector<Sophus::SE3d> getFrameStates() const { Eigen::aligned_vector<Sophus::SE3d> getFrameStates() const {
Eigen::vector<Sophus::SE3d> res; Eigen::aligned_vector<Sophus::SE3d> res;
for (const auto& kv : frame_states) { for (const auto& kv : frame_states) {
res.push_back(kv.second.getState().T_w_i); res.push_back(kv.second.getState().T_w_i);
@ -135,8 +135,8 @@ class KeypointVoEstimator : public VioEstimatorBase,
return res; return res;
} }
Eigen::vector<Sophus::SE3d> getFramePoses() const { Eigen::aligned_vector<Sophus::SE3d> getFramePoses() const {
Eigen::vector<Sophus::SE3d> res; Eigen::aligned_vector<Sophus::SE3d> res;
for (const auto& kv : frame_poses) { for (const auto& kv : frame_poses) {
res.push_back(kv.second.getPose()); res.push_back(kv.second.getPose());
@ -145,8 +145,8 @@ class KeypointVoEstimator : public VioEstimatorBase,
return res; return res;
} }
Eigen::map<int64_t, Sophus::SE3d> getAllPosesMap() const { Eigen::aligned_map<int64_t, Sophus::SE3d> getAllPosesMap() const {
Eigen::map<int64_t, Sophus::SE3d> res; Eigen::aligned_map<int64_t, Sophus::SE3d> res;
for (const auto& kv : frame_poses) { for (const auto& kv : frame_poses) {
res[kv.first] = kv.second.getPose(); res[kv.first] = kv.second.getPose();
@ -172,7 +172,7 @@ class KeypointVoEstimator : public VioEstimatorBase,
// Input // Input
Eigen::map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res; Eigen::aligned_map<int64_t, OpticalFlowResult::Ptr> prev_opt_flow_res;
std::map<int64_t, int> num_points_kf; std::map<int64_t, int> num_points_kf;

14
include/basalt/vi_estimator/landmark_database.h
View File

@ -92,8 +92,9 @@ class LandmarkDatabase {
std::vector<KeypointPosition> getLandmarksForHost( std::vector<KeypointPosition> getLandmarksForHost(
const TimeCamId& tcid) const; const TimeCamId& tcid) const;
const Eigen::map<TimeCamId, const Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>& TimeCamId, Eigen::aligned_map<
TimeCamId, Eigen::aligned_vector<KeypointObservation>>>&
getObservations() const; getObservations() const;
bool landmarkExists(int lm_id) const; bool landmarkExists(int lm_id) const;
@ -117,15 +118,16 @@ class LandmarkDatabase {
} }
private: private:
Eigen::unordered_map<int, KeypointPosition> kpts; Eigen::aligned_unordered_map<int, KeypointPosition> kpts;
Eigen::map<TimeCamId, Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>> TimeCamId,
Eigen::aligned_map<TimeCamId, Eigen::aligned_vector<KeypointObservation>>>
obs; obs;
std::unordered_map<TimeCamId, std::set<int>> host_to_kpts; std::unordered_map<TimeCamId, std::set<int>> host_to_kpts;
int num_observations = 0; int num_observations = 0;
Eigen::unordered_map<int, int> kpts_num_obs; Eigen::aligned_unordered_map<int, int> kpts_num_obs;
}; };
} // namespace basalt } // namespace basalt

17
include/basalt/vi_estimator/nfr_mapper.h
View File

@ -68,13 +68,14 @@ class NfrMapper : public BundleAdjustmentBase {
struct MapperLinearizeAbsReduce struct MapperLinearizeAbsReduce
: public BundleAdjustmentBase::LinearizeAbsReduce<AccumT> { : public BundleAdjustmentBase::LinearizeAbsReduce<AccumT> {
using RollPitchFactorConstIter = using RollPitchFactorConstIter =
Eigen::vector<RollPitchFactor>::const_iterator; Eigen::aligned_vector<RollPitchFactor>::const_iterator;
using RelPoseFactorConstIter = Eigen::vector<RelPoseFactor>::const_iterator; using RelPoseFactorConstIter =
using RelLinDataIter = Eigen::vector<RelLinData>::iterator; Eigen::aligned_vector<RelPoseFactor>::const_iterator;
using RelLinDataIter = Eigen::aligned_vector<RelLinData>::iterator;
MapperLinearizeAbsReduce( MapperLinearizeAbsReduce(
AbsOrderMap& aom, AbsOrderMap& aom,
const Eigen::map<int64_t, PoseStateWithLin>* frame_poses) const Eigen::aligned_map<int64_t, PoseStateWithLin>* frame_poses)
: BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(aom), : BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(aom),
frame_poses(frame_poses) { frame_poses(frame_poses) {
this->accum.reset(aom.total_size); this->accum.reset(aom.total_size);
@ -159,7 +160,7 @@ class NfrMapper : public BundleAdjustmentBase {
double roll_pitch_error; double roll_pitch_error;
double rel_error; double rel_error;
const Eigen::map<int64_t, PoseStateWithLin>* frame_poses; const Eigen::aligned_map<int64_t, PoseStateWithLin>* frame_poses;
}; };
NfrMapper(const basalt::Calibration<double>& calib, const VioConfig& config); NfrMapper(const basalt::Calibration<double>& calib, const VioConfig& config);
@ -172,7 +173,7 @@ class NfrMapper : public BundleAdjustmentBase {
void optimize(int num_iterations = 10); void optimize(int num_iterations = 10);
Eigen::map<int64_t, PoseStateWithLin>& getFramePoses(); Eigen::aligned_map<int64_t, PoseStateWithLin>& getFramePoses();
void computeRelPose(double& rel_error); void computeRelPose(double& rel_error);
@ -191,8 +192,8 @@ class NfrMapper : public BundleAdjustmentBase {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::vector<RollPitchFactor> roll_pitch_factors; Eigen::aligned_vector<RollPitchFactor> roll_pitch_factors;
Eigen::vector<RelPoseFactor> rel_pose_factors; Eigen::aligned_vector<RelPoseFactor> rel_pose_factors;
std::unordered_map<int64_t, OpticalFlowInput::Ptr> img_data; std::unordered_map<int64_t, OpticalFlowInput::Ptr> img_data;

12
include/basalt/vi_estimator/vio_estimator.h
View File

@ -46,15 +46,15 @@ struct VioVisualizationData {
int64_t t_ns; int64_t t_ns;
Eigen::vector<Sophus::SE3d> states; Eigen::aligned_vector<Sophus::SE3d> states;
Eigen::vector<Sophus::SE3d> frames; Eigen::aligned_vector<Sophus::SE3d> frames;
Eigen::vector<Eigen::Vector3d> points; Eigen::aligned_vector<Eigen::Vector3d> points;
std::vector<int> point_ids; std::vector<int> point_ids;
OpticalFlowResult::Ptr opt_flow_res; OpticalFlowResult::Ptr opt_flow_res;
std::vector<Eigen::vector<Eigen::Vector4d>> projections; std::vector<Eigen::aligned_vector<Eigen::Vector4d>> projections;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
@ -105,7 +105,7 @@ class VioEstimatorFactory {
}; };
double alignSVD(const std::vector<int64_t>& filter_t_ns, double alignSVD(const std::vector<int64_t>& filter_t_ns,
const Eigen::vector<Eigen::Vector3d>& filter_t_w_i, const Eigen::aligned_vector<Eigen::Vector3d>& filter_t_w_i,
const std::vector<int64_t>& gt_t_ns, const std::vector<int64_t>& gt_t_ns,
Eigen::vector<Eigen::Vector3d>& gt_t_w_i); Eigen::aligned_vector<Eigen::Vector3d>& gt_t_w_i);
} // namespace basalt } // namespace basalt

23
src/calibration/calibraiton_helper.cpp
View File

@ -51,9 +51,10 @@ namespace basalt {
template <class CamT> template <class CamT>
bool estimateTransformation( bool estimateTransformation(
const CamT &cam_calib, const Eigen::vector<Eigen::Vector2d> &corners, const CamT &cam_calib,
const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids, const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
Sophus::SE3d &T_target_camera, size_t &num_inliers) { Sophus::SE3d &T_target_camera, size_t &num_inliers) {
opengv::bearingVectors_t bearingVectors; opengv::bearingVectors_t bearingVectors;
opengv::points_t points; opengv::points_t points;
@ -140,7 +141,7 @@ void CalibHelper::detectCorners(
void CalibHelper::initCamPoses( void CalibHelper::initCamPoses(
const Calibration<double>::Ptr &calib, const Calibration<double>::Ptr &calib,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
tbb::concurrent_unordered_map<TimeCamId, CalibCornerData> &calib_corners, tbb::concurrent_unordered_map<TimeCamId, CalibCornerData> &calib_corners,
tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData> tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData>
&calib_init_poses) { &calib_init_poses) {
@ -169,13 +170,13 @@ void CalibHelper::initCamPoses(
} }
bool CalibHelper::initializeIntrinsics( bool CalibHelper::initializeIntrinsics(
const Eigen::vector<Eigen::Vector2d> &corners, const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids, const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, int cols, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
int rows, Eigen::Vector4d &init_intr) { int cols, int rows, Eigen::Vector4d &init_intr) {
// First, initialize the image center at the center of the image. // First, initialize the image center at the center of the image.
Eigen::map<int, Eigen::Vector2d> id_to_corner; Eigen::aligned_map<int, Eigen::Vector2d> id_to_corner;
for (size_t i = 0; i < corner_ids.size(); i++) { for (size_t i = 0; i < corner_ids.size(); i++) {
id_to_corner[corner_ids[i]] = corners[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) { for (size_t r = 0; r < target_rows; ++r) {
// cv::Mat P(target.cols(); 4, CV_64F); // cv::Mat P(target.cols(); 4, CV_64F);
Eigen::vector<Eigen::Vector4d> P; Eigen::aligned_vector<Eigen::Vector4d> P;
for (size_t c = 0; c < target_cols; ++c) { for (size_t c = 0; c < target_cols; ++c) {
int tag_offset = (r * target_cols + c) << 2; int tag_offset = (r * target_cols + c) << 2;
@ -302,7 +303,7 @@ bool CalibHelper::initializeIntrinsics(
void CalibHelper::computeInitialPose( void CalibHelper::computeInitialPose(
const Calibration<double>::Ptr &calib, size_t cam_id, const Calibration<double>::Ptr &calib, size_t cam_id,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
const CalibCornerData &cd, CalibInitPoseData &cp) { const CalibCornerData &cd, CalibInitPoseData &cp) {
if (cd.corners.size() < 8) { if (cd.corners.size() < 8) {
cp.num_inliers = 0; cp.num_inliers = 0;
@ -336,9 +337,9 @@ void CalibHelper::computeInitialPose(
size_t CalibHelper::computeReprojectionError( size_t CalibHelper::computeReprojectionError(
const UnifiedCamera<double> &cam_calib, const UnifiedCamera<double> &cam_calib,
const Eigen::vector<Eigen::Vector2d> &corners, const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids, const std::vector<int> &corner_ids,
const Eigen::vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d, const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
const Sophus::SE3d &T_target_camera, double &error) { const Sophus::SE3d &T_target_camera, double &error) {
size_t num_projected = 0; size_t num_projected = 0;
error = 0; error = 0;

11
src/calibration/cam_calib.cpp
View File

@ -154,13 +154,14 @@ void CamCalib::initGui() {
} }
void CamCalib::computeVign() { void CamCalib::computeVign() {
Eigen::vector<Eigen::Vector2d> optical_centers; Eigen::aligned_vector<Eigen::Vector2d> optical_centers;
for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) { for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) {
optical_centers.emplace_back( optical_centers.emplace_back(
calib_opt->calib->intrinsics[i].getParam().segment<2>(2)); calib_opt->calib->intrinsics[i].getParam().segment<2>(2));
} }
std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> reprojected_vignette2; std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
reprojected_vignette2;
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i]; int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
const std::vector<ImageData> img_vec = const std::vector<ImageData> img_vec =
@ -172,7 +173,7 @@ void CamCalib::computeVign() {
auto it = reprojected_vignette.find(tcid); auto it = reprojected_vignette.find(tcid);
if (it != reprojected_vignette.end() && img_vec[j].img.get()) { if (it != reprojected_vignette.end() && img_vec[j].img.get()) {
Eigen::vector<Eigen::Vector3d> rv; Eigen::aligned_vector<Eigen::Vector3d> rv;
rv.resize(it->second.corners_proj.size()); rv.resize(it->second.corners_proj.size());
for (size_t k = 0; k < it->second.corners_proj.size(); k++) { for (size_t k = 0; k < it->second.corners_proj.size(); k++) {
@ -314,7 +315,7 @@ void CamCalib::computeProjections() {
TimeCamId tcid(timestamp_ns, i); TimeCamId tcid(timestamp_ns, i);
ProjectedCornerData rc, rv; ProjectedCornerData rc, rv;
Eigen::vector<Eigen::Vector2d> polar_azimuthal_angle; Eigen::aligned_vector<Eigen::Vector2d> polar_azimuthal_angle;
Sophus::SE3d T_c_w_ = Sophus::SE3d T_c_w_ =
(calib_opt->getT_w_i(timestamp_ns) * calib_opt->calib->T_i_c[i]) (calib_opt->getT_w_i(timestamp_ns) * calib_opt->calib->T_i_c[i])
@ -498,7 +499,7 @@ void CamCalib::initCamIntrinsics() {
} }
} }
Eigen::vector<Eigen::Vector2i> res; Eigen::aligned_vector<Eigen::Vector2i> res;
for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { 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); res.emplace_back(img_data[i].img->w, img_data[i].img->h);

8
src/calibration/cam_imu_calib.cpp
View File

@ -313,8 +313,8 @@ void CamImuCalib::initCamImuTransform() {
} }
std::vector<int64_t> timestamps_cam; std::vector<int64_t> timestamps_cam;
Eigen::vector<Eigen::Vector3d> rot_vel_cam; Eigen::aligned_vector<Eigen::Vector3d> rot_vel_cam;
Eigen::vector<Eigen::Vector3d> rot_vel_imu; Eigen::aligned_vector<Eigen::Vector3d> rot_vel_imu;
Sophus::SO3d R_i_c0_init = calib_opt->getCamT_i_c(0).so3(); Sophus::SO3d R_i_c0_init = calib_opt->getCamT_i_c(0).so3();
@ -492,8 +492,8 @@ void CamImuCalib::initMocap() {
{ {
std::vector<int64_t> timestamps_cam; std::vector<int64_t> timestamps_cam;
Eigen::vector<Eigen::Vector3d> rot_vel_mocap; Eigen::aligned_vector<Eigen::Vector3d> rot_vel_mocap;
Eigen::vector<Eigen::Vector3d> rot_vel_imu; Eigen::aligned_vector<Eigen::Vector3d> rot_vel_imu;
Sophus::SO3d R_i_mark_init = calib_opt->mocap_calib->T_i_mark.so3(); Sophus::SO3d R_i_mark_init = calib_opt->mocap_calib->T_i_mark.so3();

6
src/calibration/vignette.cpp
View File

@ -41,9 +41,9 @@ namespace basalt {
VignetteEstimator::VignetteEstimator( VignetteEstimator::VignetteEstimator(
const VioDatasetPtr &vio_dataset, const VioDatasetPtr &vio_dataset,
const Eigen::vector<Eigen::Vector2d> &optical_centers, const Eigen::aligned_vector<Eigen::Vector2d> &optical_centers,
const Eigen::vector<Eigen::Vector2i> &resolutions, const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> const std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
&reprojected_vignette, &reprojected_vignette,
const AprilGrid &april_grid) const AprilGrid &april_grid)
: vio_dataset(vio_dataset), : vio_dataset(vio_dataset),

13
src/kitti_eval.cpp
View File

@ -66,8 +66,8 @@ inline void load(Archive& ar, std::map<std::string, T, C, A>& map) {
} // namespace cereal } // namespace cereal
Eigen::vector<Sophus::SE3d> load_poses(const std::string& path) { Eigen::aligned_vector<Sophus::SE3d> load_poses(const std::string& path) {
Eigen::vector<Sophus::SE3d> res; Eigen::aligned_vector<Sophus::SE3d> res;
std::ifstream f(path); std::ifstream f(path);
std::string line; std::string line;
@ -90,8 +90,8 @@ Eigen::vector<Sophus::SE3d> load_poses(const std::string& path) {
} }
void eval_kitti(const std::vector<double>& lengths, void eval_kitti(const std::vector<double>& lengths,
const Eigen::vector<Sophus::SE3d>& poses_gt, const Eigen::aligned_vector<Sophus::SE3d>& poses_gt,
const Eigen::vector<Sophus::SE3d>& poses_result, const Eigen::aligned_vector<Sophus::SE3d>& poses_result,
std::map<std::string, std::map<std::string, double>>& res) { std::map<std::string, std::map<std::string, double>>& res) {
auto lastFrameFromSegmentLength = [](std::vector<float>& dist, auto lastFrameFromSegmentLength = [](std::vector<float>& dist,
int first_frame, float len) { int first_frame, float len) {
@ -185,8 +185,9 @@ int main(int argc, char** argv) {
return app.exit(e); return app.exit(e);
} }
const Eigen::vector<Sophus::SE3d> poses_gt = load_poses(gt_path); const Eigen::aligned_vector<Sophus::SE3d> poses_gt = load_poses(gt_path);
const Eigen::vector<Sophus::SE3d> poses_result = load_poses(traj_path); const Eigen::aligned_vector<Sophus::SE3d> poses_result =
load_poses(traj_path);
if (poses_gt.empty() || poses_gt.size() != poses_result.size()) { if (poses_gt.empty() || poses_gt.size() != poses_result.size()) {
std::cerr << "Wrong number of poses: poses_gt " << poses_gt.size() std::cerr << "Wrong number of poses: poses_gt " << poses_gt.size()

16
src/mapper.cpp
View File

@ -74,23 +74,23 @@ using basalt::POSE_VEL_BIAS_SIZE;
Eigen::Vector3d g(0, 0, -9.81); Eigen::Vector3d g(0, 0, -9.81);
const Eigen::vector<Eigen::Vector2i> image_resolutions = {{752, 480}, const Eigen::aligned_vector<Eigen::Vector2i> image_resolutions = {{752, 480},
{752, 480}}; {752, 480}};
basalt::VioConfig vio_config; basalt::VioConfig vio_config;
basalt::NfrMapper::Ptr nrf_mapper; basalt::NfrMapper::Ptr nrf_mapper;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i;
std::vector<int64_t> gt_frame_t_ns, image_t_ns; std::vector<int64_t> gt_frame_t_ns, image_t_ns;
Eigen::vector<Eigen::Vector3d> mapper_points; Eigen::aligned_vector<Eigen::Vector3d> mapper_points;
std::vector<int> mapper_point_ids; std::vector<int> mapper_point_ids;
std::map<int64_t, basalt::MargData::Ptr> marg_data; std::map<int64_t, basalt::MargData::Ptr> marg_data;
Eigen::vector<Eigen::Vector3d> edges_vis; Eigen::aligned_vector<Eigen::Vector3d> edges_vis;
Eigen::vector<Eigen::Vector3d> roll_pitch_vis; Eigen::aligned_vector<Eigen::Vector3d> roll_pitch_vis;
Eigen::vector<Eigen::Vector3d> rel_edges_vis; Eigen::aligned_vector<Eigen::Vector3d> rel_edges_vis;
void draw_image_overlay(pangolin::View& v, size_t cam_id); void draw_image_overlay(pangolin::View& v, size_t cam_id);
void draw_scene(); void draw_scene();
@ -642,7 +642,7 @@ void optimize() {
} }
double alignButton() { double alignButton() {
Eigen::vector<Eigen::Vector3d> filter_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> filter_t_w_i;
std::vector<int64_t> filter_t_ns; std::vector<int64_t> filter_t_ns;
for (const auto& kv : nrf_mapper->getFramePoses()) { for (const auto& kv : nrf_mapper->getFramePoses()) {

30
src/mapper_sim.cpp
View File

@ -77,28 +77,28 @@ Eigen::Vector3d g(0, 0, -9.81);
std::shared_ptr<basalt::Se3Spline<5>> gt_spline; std::shared_ptr<basalt::Se3Spline<5>> gt_spline;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i; Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns; std::vector<int64_t> gt_frame_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
noisy_accel, noisy_gyro, gt_vel; gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns; std::vector<int64_t> gt_imu_t_ns;
Eigen::vector<Eigen::Vector3d> filter_points; Eigen::aligned_vector<Eigen::Vector3d> filter_points;
std::vector<int> filter_point_ids; std::vector<int> filter_point_ids;
std::map<int64_t, basalt::MargData::Ptr> marg_data; std::map<int64_t, basalt::MargData::Ptr> marg_data;
Eigen::vector<basalt::RollPitchFactor> roll_pitch_factors; Eigen::aligned_vector<basalt::RollPitchFactor> roll_pitch_factors;
Eigen::vector<basalt::RelPoseFactor> rel_pose_factors; Eigen::aligned_vector<basalt::RelPoseFactor> rel_pose_factors;
Eigen::vector<Eigen::Vector3d> edges_vis; Eigen::aligned_vector<Eigen::Vector3d> edges_vis;
Eigen::vector<Eigen::Vector3d> roll_pitch_vis; Eigen::aligned_vector<Eigen::Vector3d> roll_pitch_vis;
Eigen::vector<Eigen::Vector3d> rel_edges_vis; Eigen::aligned_vector<Eigen::Vector3d> rel_edges_vis;
Eigen::vector<Eigen::Vector3d> mapper_points; Eigen::aligned_vector<Eigen::Vector3d> mapper_points;
std::vector<int> mapper_point_ids; std::vector<int> mapper_point_ids;
basalt::NfrMapper::Ptr nrf_mapper; 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); cereal::JSONInputArchive archive(is);
int64_t t_ns; int64_t t_ns;
Eigen::vector<Sophus::SE3d> knots; Eigen::aligned_vector<Sophus::SE3d> knots;
archive(cereal::make_nvp("t_ns", t_ns)); archive(cereal::make_nvp("t_ns", t_ns));
archive(cereal::make_nvp("knots", knots)); archive(cereal::make_nvp("knots", knots));
@ -532,7 +532,7 @@ void optimize() {
void randomInc() { void randomInc() {
Sophus::Vector6d rnd = Sophus::Vector6d::Random().array().abs(); 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()) { for (auto& kv : nrf_mapper->getFramePoses()) {
Sophus::SE3d pose = random_inc * kv.second.getPose(); Sophus::SE3d pose = random_inc * kv.second.getPose();
@ -548,7 +548,7 @@ void randomYawInc() {
rnd.setZero(); rnd.setZero();
rnd[5] = std::abs(Eigen::Vector2d::Random()[0]); 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; std::cout << "random_inc\n" << random_inc.matrix() << std::endl;
@ -562,7 +562,7 @@ void randomYawInc() {
} }
double alignButton() { double alignButton() {
Eigen::vector<Eigen::Vector3d> filter_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> filter_t_w_i;
std::vector<int64_t> filter_t_ns; std::vector<int64_t> filter_t_ns;
for (const auto& kv : nrf_mapper->getFramePoses()) { for (const auto& kv : nrf_mapper->getFramePoses()) {

18
src/mapper_sim_naive.cpp
View File

@ -88,12 +88,12 @@ std::mt19937 gen{1};
basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9));
Eigen::vector<Eigen::Vector3d> gt_points; Eigen::aligned_vector<Eigen::Vector3d> gt_points;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i; Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns, kf_t_ns; std::vector<int64_t> gt_frame_t_ns, kf_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
noisy_accel, noisy_gyro, gt_vel; gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns; std::vector<int64_t> gt_imu_t_ns;
std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations; std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
@ -356,7 +356,7 @@ int main(int argc, char** argv) {
// t4.join(); // t4.join();
if (!result_path.empty()) { if (!result_path.empty()) {
Eigen::vector<Eigen::Vector3d> vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
auto it = vis_map.find(kf_t_ns.back()); auto it = vis_map.find(kf_t_ns.back());
@ -528,7 +528,7 @@ void gen_data() {
cereal::JSONInputArchive archive(is); cereal::JSONInputArchive archive(is);
int64_t t_ns; int64_t t_ns;
Eigen::vector<Sophus::SE3d> knots; Eigen::aligned_vector<Sophus::SE3d> knots;
archive(cereal::make_nvp("t_ns", t_ns)); archive(cereal::make_nvp("t_ns", t_ns));
archive(cereal::make_nvp("knots", knots)); archive(cereal::make_nvp("knots", knots));
@ -571,7 +571,7 @@ void gen_data() {
mdl.start(marg_data_path); mdl.start(marg_data_path);
Eigen::map<int64_t, Sophus::SE3d> tmp_poses; Eigen::aligned_map<int64_t, Sophus::SE3d> tmp_poses;
while (true) { while (true) {
basalt::MargData::Ptr data; basalt::MargData::Ptr data;
@ -747,7 +747,7 @@ bool next_step() {
} }
void alignButton() { void alignButton() {
Eigen::vector<Eigen::Vector3d> vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
auto it = vis_map.find(kf_t_ns.back()); auto it = vis_map.find(kf_t_ns.back());

4
src/opt_flow.cpp
View File

@ -293,8 +293,8 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (observations.count(t_ns) > 0) { if (observations.count(t_ns) > 0) {
const Eigen::map<basalt::KeypointId, Eigen::AffineCompact2f>& kp_map = const Eigen::aligned_map<basalt::KeypointId, Eigen::AffineCompact2f>&
observations.at(t_ns)->observations[cam_id]; kp_map = observations.at(t_ns)->observations[cam_id];
for (const auto& kv : kp_map) { for (const auto& kv : kp_map) {
Eigen::MatrixXf transformed_patch = Eigen::MatrixXf transformed_patch =

5
src/rs_t265_vio.cpp
View File

@ -100,7 +100,7 @@ tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue; tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
std::vector<int64_t> vio_t_ns; std::vector<int64_t> vio_t_ns;
Eigen::vector<Eigen::Vector3d> vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
std::string marg_data_path; std::string marg_data_path;
@ -408,7 +408,8 @@ void draw_scene() {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor3ubv(cam_color); glColor3ubv(cam_color);
Eigen::vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(), vio_t_w_i.end()); Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
vio_t_w_i.end());
pangolin::glDrawLineStrip(sub_gt); pangolin::glDrawLineStrip(sub_gt);
if (curr_vis_data.get()) { if (curr_vis_data.get()) {

14
src/time_alignment.cpp
View File

@ -55,11 +55,11 @@ basalt::Calibration<double> calib;
basalt::MocapCalibration<double> mocap_calib; basalt::MocapCalibration<double> mocap_calib;
// Linear time version // Linear time version
double compute_error(int64_t offset, double compute_error(
const std::vector<int64_t> &gyro_timestamps, int64_t offset, const std::vector<int64_t> &gyro_timestamps,
const Eigen::vector<Eigen::Vector3d> &gyro_data, const Eigen::aligned_vector<Eigen::Vector3d> &gyro_data,
const std::vector<int64_t> &mocap_rot_vel_timestamps, const std::vector<int64_t> &mocap_rot_vel_timestamps,
const Eigen::vector<Eigen::Vector3d> &mocap_rot_vel_data) { const Eigen::aligned_vector<Eigen::Vector3d> &mocap_rot_vel_data) {
double error = 0; double error = 0;
int num_points = 0; int num_points = 0;
@ -177,10 +177,10 @@ int main(int argc, char **argv) {
vio_dataset = dataset_io->get_data(); vio_dataset = dataset_io->get_data();
std::vector<int64_t> gyro_timestamps; std::vector<int64_t> gyro_timestamps;
Eigen::vector<Eigen::Vector3d> gyro_data; Eigen::aligned_vector<Eigen::Vector3d> gyro_data;
std::vector<int64_t> mocap_rot_vel_timestamps; std::vector<int64_t> mocap_rot_vel_timestamps;
Eigen::vector<Eigen::Vector3d> mocap_rot_vel_data; Eigen::aligned_vector<Eigen::Vector3d> mocap_rot_vel_data;
// Apply calibration to gyro // Apply calibration to gyro
{ {

7
src/utils/keypoints.cpp
View File

@ -154,9 +154,10 @@ void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
} }
} }
void detectKeypoints(const basalt::Image<const uint16_t>& img_raw, void detectKeypoints(
KeypointsData& kd, int PATCH_SIZE, int num_points_cell, const basalt::Image<const uint16_t>& img_raw, KeypointsData& kd,
const Eigen::vector<Eigen::Vector2d>& current_points) { int PATCH_SIZE, int num_points_cell,
const Eigen::aligned_vector<Eigen::Vector2d>& current_points) {
kd.corners.clear(); kd.corners.clear();
kd.corner_angles.clear(); kd.corner_angles.clear();
kd.corner_descriptors.clear(); kd.corner_descriptors.clear();

14
src/vi_estimator/ba_base.cpp
View File

@ -227,9 +227,10 @@ void BundleAdjustmentBase::computeError(
} }
void BundleAdjustmentBase::linearizeHelper( void BundleAdjustmentBase::linearizeHelper(
Eigen::vector<RelLinData>& rld_vec, Eigen::aligned_vector<RelLinData>& rld_vec,
const Eigen::map<TimeCamId, const Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>& TimeCamId, Eigen::aligned_map<
TimeCamId, Eigen::aligned_vector<KeypointObservation>>>&
obs_to_lin, obs_to_lin,
double& error) const { double& error) const {
error = 0; error = 0;
@ -422,7 +423,8 @@ void BundleAdjustmentBase::linearizeRel(const RelLinData& rld,
} }
void BundleAdjustmentBase::get_current_points( void BundleAdjustmentBase::get_current_points(
Eigen::vector<Eigen::Vector3d>& points, std::vector<int>& ids) const { Eigen::aligned_vector<Eigen::Vector3d>& points,
std::vector<int>& ids) const {
points.clear(); points.clear();
ids.clear(); ids.clear();
@ -632,8 +634,8 @@ void BundleAdjustmentBase::computeMargPriorError(
Eigen::VectorXd BundleAdjustmentBase::checkNullspace( Eigen::VectorXd BundleAdjustmentBase::checkNullspace(
const Eigen::MatrixXd& H, const Eigen::VectorXd& b, const Eigen::MatrixXd& H, const Eigen::VectorXd& b,
const AbsOrderMap& order, const AbsOrderMap& order,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& frame_states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::map<int64_t, PoseStateWithLin>& frame_poses) { const Eigen::aligned_map<int64_t, PoseStateWithLin>& frame_poses) {
BASALT_ASSERT(size_t(H.cols()) == order.total_size); BASALT_ASSERT(size_t(H.cols()) == order.total_size);
size_t marg_size = order.total_size; size_t marg_size = order.total_size;

13
src/vi_estimator/keypoint_vio.cpp
View File

@ -573,8 +573,9 @@ void KeypointVioEstimator::marginalize(
{ {
// Linearize points // Linearize points
Eigen::map<TimeCamId, Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>> TimeCamId, Eigen::aligned_map<
TimeCamId, Eigen::aligned_vector<KeypointObservation>>>
obs_to_lin; obs_to_lin;
for (auto it = lmdb.getObservations().cbegin(); for (auto it = lmdb.getObservations().cbegin();
@ -590,7 +591,7 @@ void KeypointVioEstimator::marginalize(
} }
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::aligned_vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs_to_lin, rld_error); linearizeHelper(rld_vec, obs_to_lin, rld_error);
@ -791,13 +792,13 @@ void KeypointVioEstimator::optimize() {
auto t1 = std::chrono::high_resolution_clock::now(); auto t1 = std::chrono::high_resolution_clock::now();
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::aligned_vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt( BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt(
aom); aom);
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range( tbb::blocked_range<Eigen::aligned_vector<RelLinData>::iterator> range(
rld_vec.begin(), rld_vec.end()); rld_vec.begin(), rld_vec.end());
tbb::parallel_reduce(range, lopt); tbb::parallel_reduce(range, lopt);
@ -998,7 +999,7 @@ void KeypointVioEstimator::optimize() {
} // namespace basalt } // namespace basalt
void KeypointVioEstimator::computeProjections( void KeypointVioEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const { std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& data) const {
for (const auto& kv : lmdb.getObservations()) { for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first; const TimeCamId& tcid_h = kv.first;

8
src/vi_estimator/keypoint_vio_linearize.cpp
View File

@ -40,8 +40,8 @@ namespace basalt {
void KeypointVioEstimator::linearizeAbsIMU( void KeypointVioEstimator::linearizeAbsIMU(
const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b,
double& imu_error, double& bg_error, double& ba_error, double& imu_error, double& bg_error, double& ba_error,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas, const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& gyro_bias_weight,
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) {
imu_error = 0; imu_error = 0;
@ -179,8 +179,8 @@ void KeypointVioEstimator::linearizeAbsIMU(
void KeypointVioEstimator::computeImuError( void KeypointVioEstimator::computeImuError(
const AbsOrderMap& aom, double& imu_error, double& bg_error, const AbsOrderMap& aom, double& imu_error, double& bg_error,
double& ba_error, double& ba_error,
const Eigen::map<int64_t, PoseVelBiasStateWithLin>& states, const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::map<int64_t, IntegratedImuMeasurement>& imu_meas, const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& gyro_bias_weight,
const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) {
imu_error = 0; imu_error = 0;

14
src/vi_estimator/keypoint_vo.cpp
View File

@ -487,8 +487,10 @@ void KeypointVoEstimator::marginalize(
{ {
// Linearize points // Linearize points
Eigen::map<TimeCamId, Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>> TimeCamId,
Eigen::aligned_map<TimeCamId,
Eigen::aligned_vector<KeypointObservation>>>
obs_to_lin; obs_to_lin;
for (auto it = lmdb.getObservations().cbegin(); for (auto it = lmdb.getObservations().cbegin();
@ -503,7 +505,7 @@ void KeypointVoEstimator::marginalize(
} }
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::aligned_vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, obs_to_lin, rld_error); linearizeHelper(rld_vec, obs_to_lin, rld_error);
@ -651,13 +653,13 @@ void KeypointVoEstimator::optimize() {
auto t1 = std::chrono::high_resolution_clock::now(); auto t1 = std::chrono::high_resolution_clock::now();
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::aligned_vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt( BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt(
aom); aom);
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range( tbb::blocked_range<Eigen::aligned_vector<RelLinData>::iterator> range(
rld_vec.begin(), rld_vec.end()); rld_vec.begin(), rld_vec.end());
tbb::parallel_reduce(range, lopt); tbb::parallel_reduce(range, lopt);
@ -836,7 +838,7 @@ void KeypointVoEstimator::optimize() {
} // namespace basalt } // namespace basalt
void KeypointVoEstimator::computeProjections( void KeypointVoEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const { std::vector<Eigen::aligned_vector<Eigen::Vector4d>>& data) const {
for (const auto& kv : lmdb.getObservations()) { for (const auto& kv : lmdb.getObservations()) {
const TimeCamId& tcid_h = kv.first; const TimeCamId& tcid_h = kv.first;

5
src/vi_estimator/landmark_database.cpp
View File

@ -145,8 +145,9 @@ const KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) const {
return kpts.at(lm_id); return kpts.at(lm_id);
} }
const Eigen::map<TimeCamId, const Eigen::aligned_map<
Eigen::map<TimeCamId, Eigen::vector<KeypointObservation> > > TimeCamId,
Eigen::aligned_map<TimeCamId, Eigen::aligned_vector<KeypointObservation> > >
&LandmarkDatabase::getObservations() const { &LandmarkDatabase::getObservations() const {
return obs; return obs;
} }

14
src/vi_estimator/nfr_mapper.cpp
View File

@ -251,7 +251,7 @@ void NfrMapper::optimize(int num_iterations) {
auto t1 = std::chrono::high_resolution_clock::now(); auto t1 = std::chrono::high_resolution_clock::now();
double rld_error; double rld_error;
Eigen::vector<RelLinData> rld_vec; Eigen::aligned_vector<RelLinData> rld_vec;
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error); linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
// SparseHashAccumulator<double> accum; // SparseHashAccumulator<double> accum;
@ -269,12 +269,12 @@ void NfrMapper::optimize(int num_iterations) {
MapperLinearizeAbsReduce<SparseHashAccumulator<double>> lopt(aom, MapperLinearizeAbsReduce<SparseHashAccumulator<double>> lopt(aom,
&frame_poses); &frame_poses);
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range( tbb::blocked_range<Eigen::aligned_vector<RelLinData>::iterator> range(
rld_vec.begin(), rld_vec.end()); rld_vec.begin(), rld_vec.end());
tbb::blocked_range<Eigen::vector<RollPitchFactor>::const_iterator> range1( tbb::blocked_range<Eigen::aligned_vector<RollPitchFactor>::const_iterator>
roll_pitch_factors.begin(), roll_pitch_factors.end()); range1(roll_pitch_factors.begin(), roll_pitch_factors.end());
tbb::blocked_range<Eigen::vector<RelPoseFactor>::const_iterator> range2( tbb::blocked_range<Eigen::aligned_vector<RelPoseFactor>::const_iterator>
rel_pose_factors.begin(), rel_pose_factors.end()); range2(rel_pose_factors.begin(), rel_pose_factors.end());
tbb::parallel_reduce(range, lopt); tbb::parallel_reduce(range, lopt);
@ -420,7 +420,7 @@ void NfrMapper::optimize(int num_iterations) {
} }
} }
Eigen::map<int64_t, PoseStateWithLin>& NfrMapper::getFramePoses() { Eigen::aligned_map<int64_t, PoseStateWithLin>& NfrMapper::getFramePoses() {
return frame_poses; return frame_poses;
} }

8
src/vi_estimator/vio_estimator.cpp
View File

@ -55,11 +55,11 @@ VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
} }
double alignSVD(const std::vector<int64_t>& filter_t_ns, double alignSVD(const std::vector<int64_t>& filter_t_ns,
const Eigen::vector<Eigen::Vector3d>& filter_t_w_i, const Eigen::aligned_vector<Eigen::Vector3d>& filter_t_w_i,
const std::vector<int64_t>& gt_t_ns, const std::vector<int64_t>& gt_t_ns,
Eigen::vector<Eigen::Vector3d>& gt_t_w_i) { Eigen::aligned_vector<Eigen::Vector3d>& gt_t_w_i) {
Eigen::vector<Eigen::Vector3d> est_associations; Eigen::aligned_vector<Eigen::Vector3d> est_associations;
Eigen::vector<Eigen::Vector3d> gt_associations; Eigen::aligned_vector<Eigen::Vector3d> gt_associations;
for (size_t i = 0; i < filter_t_w_i.size(); i++) { for (size_t i = 0; i < filter_t_w_i.size(); i++) {
int64_t t_ns = filter_t_ns[i]; int64_t t_ns = filter_t_ns[i];

14
src/vio.cpp
View File

@ -121,11 +121,11 @@ tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue; tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
std::vector<int64_t> vio_t_ns; std::vector<int64_t> vio_t_ns;
Eigen::vector<Eigen::Vector3d> vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
Eigen::vector<Sophus::SE3d> vio_T_w_i; Eigen::aligned_vector<Sophus::SE3d> vio_T_w_i;
std::vector<int64_t> gt_t_ns; std::vector<int64_t> gt_t_ns;
Eigen::vector<Eigen::Vector3d> gt_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> gt_t_w_i;
std::string marg_data_path; std::string marg_data_path;
size_t last_frame_processed = 0; 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); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (it != vis_map.end()) { if (it != vis_map.end()) {
const Eigen::map<basalt::KeypointId, Eigen::AffineCompact2f>& kp_map = const Eigen::aligned_map<basalt::KeypointId, Eigen::AffineCompact2f>&
it->second->opt_flow_res->observations[cam_id]; kp_map = it->second->opt_flow_res->observations[cam_id];
for (const auto& kv : kp_map) { for (const auto& kv : kp_map) {
Eigen::MatrixXf transformed_patch = Eigen::MatrixXf transformed_patch =
@ -678,8 +678,8 @@ void draw_scene(pangolin::View& view) {
glColor3ubv(cam_color); glColor3ubv(cam_color);
if (!vio_t_w_i.empty()) { if (!vio_t_w_i.empty()) {
Eigen::vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(), Eigen::aligned_vector<Eigen::Vector3d> sub_gt(
vio_t_w_i.begin() + show_frame); vio_t_w_i.begin(), vio_t_w_i.begin() + show_frame);
pangolin::glDrawLineStrip(sub_gt); pangolin::glDrawLineStrip(sub_gt);
} }

12
src/vio_sim.cpp
View File

@ -97,12 +97,12 @@ std::normal_distribution<> obs_noise_dist{0, obs_std_dev};
basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9)); basalt::Se3Spline<5> gt_spline(int64_t(knot_time * 1e9));
Eigen::vector<Eigen::Vector3d> gt_points; Eigen::aligned_vector<Eigen::Vector3d> gt_points;
Eigen::vector<Sophus::SE3d> gt_frame_T_w_i; Eigen::aligned_vector<Sophus::SE3d> gt_frame_T_w_i;
Eigen::vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i; Eigen::aligned_vector<Eigen::Vector3d> gt_frame_t_w_i, vio_t_w_i;
std::vector<int64_t> gt_frame_t_ns; std::vector<int64_t> gt_frame_t_ns;
Eigen::vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias, gt_gyro_bias, Eigen::aligned_vector<Eigen::Vector3d> gt_accel, gt_gyro, gt_accel_bias,
noisy_accel, noisy_gyro, gt_vel; gt_gyro_bias, noisy_accel, noisy_gyro, gt_vel;
std::vector<int64_t> gt_imu_t_ns; std::vector<int64_t> gt_imu_t_ns;
std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations; std::map<basalt::TimeCamId, basalt::SimObservations> gt_observations;
@ -730,7 +730,7 @@ void gen_data() {
int64_t t_ns = gt_spline.getDtNs(); int64_t t_ns = gt_spline.getDtNs();
Eigen::vector<Sophus::SE3d> knots; Eigen::aligned_vector<Sophus::SE3d> knots;
for (size_t i = 0; i < gt_spline.numKnots(); i++) { for (size_t i = 0; i < gt_spline.numKnots(); i++) {
knots.push_back(gt_spline.getKnot(i)); knots.push_back(gt_spline.getKnot(i));
} }

18
test/src/test_nfr.cpp
View File

@ -22,11 +22,11 @@ std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev};
std::normal_distribution<> accel_noise_dist{0, accel_std_dev}; std::normal_distribution<> accel_noise_dist{0, accel_std_dev};
TEST(PreIntegrationTestSuite, RelPoseTest) { TEST(PreIntegrationTestSuite, RelPoseTest) {
Sophus::SE3d T_w_i = Sophus::expd(Sophus::Vector6d::Random()); Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_w_j = Sophus::expd(Sophus::Vector6d::Random()); Sophus::SE3d T_w_j = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_i_j = Sophus::SE3d T_i_j = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) *
Sophus::expd(Sophus::Vector6d::Random() / 100) * T_w_i.inverse() * T_w_j; T_w_i.inverse() * T_w_j;
Sophus::Matrix6d d_res_d_T_w_i, d_res_d_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); 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) { 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; Eigen::Vector3d pos = T_w_i.translation() + Eigen::Vector3d::Random() / 10;
@ -84,12 +84,12 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) {
} }
TEST(PreIntegrationTestSuite, YawTest) { 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 = Eigen::Vector3d yaw_dir_body =
T_w_i.so3().inverse() * Eigen::Vector3d::UnitX(); 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<double, 1, 6> d_res_d_T_w_i; Sophus::Matrix<double, 1, 6> d_res_d_T_w_i;
basalt::yawError(T_w_i, yaw_dir_body, &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) { 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(); 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<double, 2, 6> d_res_d_T_w_i; Sophus::Matrix<double, 2, 6> d_res_d_T_w_i;
basalt::rollPitchError(T_w_i, R_w_i, &d_res_d_T_w_i); basalt::rollPitchError(T_w_i, R_w_i, &d_res_d_T_w_i);

37
test/src/test_vio.cpp
View File

@ -74,7 +74,7 @@ TEST(VioTestSuite, ImuNullspace2Test) {
state1.t_ns = imu_meas.get_dt_ns(); state1.t_ns = imu_meas.get_dt_ns();
state1.T_w_i = gt_spline.pose(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()) + state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) +
Sophus::Vector3d::Random() / 10; Sophus::Vector3d::Random() / 10;
state1.bias_gyro = bg; state1.bias_gyro = bg;
@ -86,9 +86,9 @@ TEST(VioTestSuite, ImuNullspace2Test) {
Eigen::Vector3d accel_weight; Eigen::Vector3d accel_weight;
accel_weight.setConstant(1e6); accel_weight.setConstant(1e6);
Eigen::map<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec; Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states; Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, basalt::PoseStateWithLin> frame_poses; Eigen::aligned_map<int64_t, basalt::PoseStateWithLin> frame_poses;
imu_meas_vec[state0.t_ns] = imu_meas; imu_meas_vec[state0.t_ns] = imu_meas;
frame_states[state0.t_ns] = state0; frame_states[state0.t_ns] = state0;
@ -227,7 +227,7 @@ TEST(VioTestSuite, ImuNullspace3Test) {
state1.t_ns = imu_meas1.get_dt_ns(); state1.t_ns = imu_meas1.get_dt_ns();
state1.T_w_i = gt_spline.pose(state1.t_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 = state1.vel_w_i =
gt_spline.transVelWorld(state1.t_ns) + Sophus::Vector3d::Random() / 10; gt_spline.transVelWorld(state1.t_ns) + Sophus::Vector3d::Random() / 10;
state1.bias_gyro = bg; 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_ns = imu_meas1.get_dt_ns() + imu_meas2.get_dt_ns();
state2.T_w_i = gt_spline.pose(state2.t_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 = state2.vel_w_i =
gt_spline.transVelWorld(state2.t_ns) + Sophus::Vector3d::Random() / 10; gt_spline.transVelWorld(state2.t_ns) + Sophus::Vector3d::Random() / 10;
state2.bias_gyro = bg; state2.bias_gyro = bg;
@ -247,9 +247,9 @@ TEST(VioTestSuite, ImuNullspace3Test) {
Eigen::Vector3d accel_weight; Eigen::Vector3d accel_weight;
accel_weight.setConstant(1e6); accel_weight.setConstant(1e6);
Eigen::map<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec; Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states; Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::map<int64_t, basalt::PoseStateWithLin> frame_poses; Eigen::aligned_map<int64_t, basalt::PoseStateWithLin> frame_poses;
imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1; imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1;
imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2; imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2;
@ -287,11 +287,11 @@ TEST(VioTestSuite, ImuNullspace3Test) {
} }
TEST(VioTestSuite, RelPoseTest) { TEST(VioTestSuite, RelPoseTest) {
Sophus::SE3d T_w_i_h = Sophus::expd(Sophus::Vector6d::Random()); Sophus::SE3d T_w_i_h = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_w_i_t = Sophus::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_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
Sophus::SE3d T_i_c_t = Sophus::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; 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, basalt::KeypointVioEstimator::computeRelPose(T_w_h_new, T_i_c_h,
T_w_i_t, T_i_c_t); 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); x0);
} }
@ -328,7 +328,7 @@ TEST(VioTestSuite, RelPoseTest) {
Sophus::SE3d T_t_h_sophus_new = Sophus::SE3d T_t_h_sophus_new =
basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h, basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h,
T_w_t_new, T_i_c_t); 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); x0);
} }
@ -345,8 +345,8 @@ TEST(VioTestSuite, LinearizePointsTest) {
kpt_pos.dir = basalt::StereographicParam<double>::project(point3d); kpt_pos.dir = basalt::StereographicParam<double>::project(point3d);
kpt_pos.id = 0.1231231; kpt_pos.id = 0.1231231;
Sophus::SE3d T_w_h = Sophus::expd(Sophus::Vector6d::Random() / 100); Sophus::SE3d T_w_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 100);
Sophus::SE3d T_w_t = Sophus::expd(Sophus::Vector6d::Random() / 100); Sophus::SE3d T_w_t = Sophus::se3_expd(Sophus::Vector6d::Random() / 100);
T_w_t.translation()[0] += 0.1; T_w_t.translation()[0] += 0.1;
Sophus::SE3d T_t_h_sophus = T_w_t.inverse() * T_w_h; Sophus::SE3d T_t_h_sophus = T_w_t.inverse() * T_w_h;
@ -374,7 +374,8 @@ TEST(VioTestSuite, LinearizePointsTest) {
test_jacobian( test_jacobian(
"d_res_d_xi", d_res_d_xi, "d_res_d_xi", d_res_d_xi,
[&](const Sophus::Vector6d& x) { [&](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; Eigen::Vector2d res;
basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos, basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos,

4
thirdparty/apriltag/include/basalt/utils/apriltag.h vendored
View File

@ -14,9 +14,9 @@ class ApriltagDetector {
~ApriltagDetector(); ~ApriltagDetector();
void detectTags(basalt::ManagedImage<uint16_t>& img_raw, void detectTags(basalt::ManagedImage<uint16_t>& img_raw,
Eigen::vector<Eigen::Vector2d>& corners, Eigen::aligned_vector<Eigen::Vector2d>& corners,
std::vector<int>& ids, std::vector<double>& radii, std::vector<int>& ids, std::vector<double>& radii,
Eigen::vector<Eigen::Vector2d>& corners_rejected, Eigen::aligned_vector<Eigen::Vector2d>& corners_rejected,
std::vector<int>& ids_rejected, std::vector<int>& ids_rejected,
std::vector<double>& radii_rejected); std::vector<double>& radii_rejected);

4
thirdparty/apriltag/src/apriltag.cpp vendored
View File

@ -39,9 +39,9 @@ ApriltagDetector::~ApriltagDetector() { delete data; }
void ApriltagDetector::detectTags( void ApriltagDetector::detectTags(
basalt::ManagedImage<uint16_t>& img_raw, basalt::ManagedImage<uint16_t>& img_raw,
Eigen::vector<Eigen::Vector2d>& corners, std::vector<int>& ids, Eigen::aligned_vector<Eigen::Vector2d>& corners, std::vector<int>& ids,
std::vector<double>& radii, std::vector<double>& radii,
Eigen::vector<Eigen::Vector2d>& corners_rejected, Eigen::aligned_vector<Eigen::Vector2d>& corners_rejected,
std::vector<int>& ids_rejected, std::vector<double>& radii_rejected) { std::vector<int>& ids_rejected, std::vector<double>& radii_rejected) {
corners.clear(); corners.clear();
ids.clear(); ids.clear();

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit ff561d2369d8dd159ff1e7316e451bec41544ebe Subproject commit 7847c4699607eaca370427ae4e2e840c5b502526