Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use smart pointers instead of raw pointers #96

Closed
wants to merge 1 commit into from
Closed

Use smart pointers instead of raw pointers #96

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 3 additions & 1 deletion ov_core/src/track/TrackBase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,8 @@ namespace ov_core {
currid = (size_t) numaruco + 1;
}

virtual ~TrackBase() { }


/**
* @brief Given a the camera intrinsic values, this will set what we should normalize points with.
Expand Down Expand Up @@ -372,4 +374,4 @@ namespace ov_core {

}

#endif /* OV_CORE_TRACK_BASE_H */
#endif /* OV_CORE_TRACK_BASE_H */
50 changes: 25 additions & 25 deletions ov_msckf/src/core/VioManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ VioManager::VioManager(VioManagerOptions& params_) {
params.print_trackers();

// Create the state!!
state = new State(params.state_options);
state = std::unique_ptr<State>(new State(params.state_options));

// Timeoffset from camera to IMU
Eigen::VectorXd temp_camimu_dt;
Expand Down Expand Up @@ -104,32 +104,32 @@ VioManager::VioManager(VioManagerOptions& params_) {

// Lets make a feature extractor
if(params.use_klt) {
trackFEATS = new TrackKLT(params.num_pts,state->_options.max_aruco_features,params.fast_threshold,params.grid_x,params.grid_y,params.min_px_dist);
trackFEATS = std::unique_ptr<TrackBase>(new TrackKLT(params.num_pts,state->_options.max_aruco_features,params.fast_threshold,params.grid_x,params.grid_y,params.min_px_dist));
trackFEATS->set_calibration(params.camera_intrinsics, params.camera_fisheye);
} else {
trackFEATS = new TrackDescriptor(params.num_pts,state->_options.max_aruco_features,params.fast_threshold,params.grid_x,params.grid_y,params.knn_ratio);
trackFEATS = std::unique_ptr<TrackBase>(new TrackDescriptor(params.num_pts,state->_options.max_aruco_features,params.fast_threshold,params.grid_x,params.grid_y,params.knn_ratio));
trackFEATS->set_calibration(params.camera_intrinsics, params.camera_fisheye);
}

// Initialize our aruco tag extractor
if(params.use_aruco) {
trackARUCO = new TrackAruco(state->_options.max_aruco_features, params.downsize_aruco);
trackARUCO = std::unique_ptr<TrackBase>(new TrackAruco(state->_options.max_aruco_features, params.downsize_aruco));
trackARUCO->set_calibration(params.camera_intrinsics, params.camera_fisheye);
}

// Initialize our state propagator
propagator = new Propagator(params.imu_noises, params.gravity);
propagator = std::unique_ptr<Propagator>(new Propagator(params.imu_noises, params.gravity));

// Our state initialize
initializer = new InertialInitializer(params.gravity,params.init_window_time,params.init_imu_thresh);
initializer = std::unique_ptr<InertialInitializer>(new InertialInitializer(params.gravity,params.init_window_time,params.init_imu_thresh));

// Make the updater!
updaterMSCKF = new UpdaterMSCKF(params.msckf_options,params.featinit_options);
updaterSLAM = new UpdaterSLAM(params.slam_options,params.aruco_options,params.featinit_options);
updaterMSCKF = std::unique_ptr<UpdaterMSCKF>(new UpdaterMSCKF(params.msckf_options,params.featinit_options));
updaterSLAM = std::unique_ptr<UpdaterSLAM>(new UpdaterSLAM(params.slam_options,params.aruco_options,params.featinit_options));

// If we are using zero velocity updates, then create the updater
if(params.try_zupt) {
updaterZUPT = new UpdaterZeroVelocity(params.zupt_options,params.imu_noises,params.gravity,params.zupt_max_velocity,params.zupt_noise_multiplier);
updaterZUPT = std::unique_ptr<UpdaterZeroVelocity>(new UpdaterZeroVelocity(params.zupt_options,params.imu_noises,params.gravity,params.zupt_max_velocity,params.zupt_noise_multiplier));
}

}
Expand Down Expand Up @@ -172,7 +172,7 @@ void VioManager::feed_measurement_monocular(double timestamp, cv::Mat& img0, siz

// Check if we should do zero-velocity, if so update the state with it
if(is_initialized_vio && updaterZUPT != nullptr) {
did_zupt_update = updaterZUPT->try_update(state, timestamp);
did_zupt_update = updaterZUPT->try_update(state.get(), timestamp);
if(did_zupt_update) {
cv::Mat img_outtemp0;
cv::cvtColor(img0, img_outtemp0, CV_GRAY2RGB);
Expand Down Expand Up @@ -226,7 +226,7 @@ void VioManager::feed_measurement_stereo(double timestamp, cv::Mat& img0, cv::Ma

// Check if we should do zero-velocity, if so update the state with it
if(is_initialized_vio && updaterZUPT != nullptr) {
did_zupt_update = updaterZUPT->try_update(state, timestamp);
did_zupt_update = updaterZUPT->try_update(state.get(), timestamp);
if(did_zupt_update) {
cv::Mat img_outtemp0, img_outtemp1;
cv::cvtColor(img0, img_outtemp0, CV_GRAY2RGB);
Expand All @@ -244,8 +244,9 @@ void VioManager::feed_measurement_stereo(double timestamp, cv::Mat& img0, cv::Ma
if(params.use_stereo) {
trackFEATS->feed_stereo(timestamp, img0, img1, cam_id0, cam_id1);
} else {
boost::thread t_l = boost::thread(&TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img0), boost::ref(cam_id0));
boost::thread t_r = boost::thread(&TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img1), boost::ref(cam_id1));
// Calling trackFEATS.get() is safe because `this` is still in scope when we join, hence trackFEATS is alive for at least that long
boost::thread t_l = boost::thread(&TrackBase::feed_monocular, trackFEATS.get(), boost::ref(timestamp), boost::ref(img0), boost::ref(cam_id0));
boost::thread t_r = boost::thread(&TrackBase::feed_monocular, trackFEATS.get(), boost::ref(timestamp), boost::ref(img1), boost::ref(cam_id1));
t_l.join();
t_r.join();
}
Expand Down Expand Up @@ -278,17 +279,18 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
rT1 = boost::posix_time::microsec_clock::local_time();

// Check if we actually have a simulated tracker
TrackSIM *trackSIM = dynamic_cast<TrackSIM*>(trackFEATS);
// Calling trackFEATS.get() is safe because `this` (and thus `trackFEATS`) outlives `trackSIM`
TrackSIM *trackSIM = dynamic_cast<TrackSIM*>(trackFEATS.get());
if(trackSIM == nullptr) {
//delete trackFEATS; //(fix this error in the future)
trackFEATS = new TrackSIM(state->_options.max_aruco_features);
trackFEATS = std::unique_ptr<TrackSIM>(new TrackSIM(state->_options.max_aruco_features));
trackFEATS->set_calibration(params.camera_intrinsics, params.camera_fisheye);
printf(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET);
}

// Check if we should do zero-velocity, if so update the state with it
if(is_initialized_vio && updaterZUPT != nullptr) {
did_zupt_update = updaterZUPT->try_update(state, timestamp);
did_zupt_update = updaterZUPT->try_update(state.get(), timestamp);
if(did_zupt_update) {
int max_width = -1;
int max_height = -1;
Expand All @@ -311,8 +313,6 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
}
}

// Cast the tracker to our simulation tracker
trackSIM = dynamic_cast<TrackSIM*>(trackFEATS);
trackSIM->set_width_height(params.camera_wh);

// Feed our simulation tracker
Expand Down Expand Up @@ -399,7 +399,7 @@ void VioManager::do_feature_propagate_update(double timestamp) {

// Propagate the state forward to the current update time
// Also augment it with a new clone!
propagator->propagate_and_clone(state, timestamp);
propagator->propagate_and_clone(state.get(), timestamp);
rT3 = boost::posix_time::microsec_clock::local_time();

// If we have not reached max clones, we should just return...
Expand Down Expand Up @@ -505,7 +505,7 @@ void VioManager::do_feature_propagate_update(double timestamp) {
// Lets marginalize out all old SLAM features here
// These are ones that where not successfully tracked into the current frame
// We do *NOT* marginalize out our aruco tags
StateHelper::marginalize_slam(state);
StateHelper::marginalize_slam(state.get());

// Separate our SLAM features into new ones, and old ones
std::vector<Feature*> feats_slam_DELAYED, feats_slam_UPDATE;
Expand Down Expand Up @@ -535,7 +535,7 @@ void VioManager::do_feature_propagate_update(double timestamp) {
// NOTE: this should only really be used if you want to track a lot of features, or have limited computational resources
if((int)featsup_MSCKF.size() > state->_options.max_msckf_in_update)
featsup_MSCKF.erase(featsup_MSCKF.begin(), featsup_MSCKF.end()-state->_options.max_msckf_in_update);
updaterMSCKF->update(state, featsup_MSCKF);
updaterMSCKF->update(state.get(), featsup_MSCKF);
rT4 = boost::posix_time::microsec_clock::local_time();

// Perform SLAM delay init and update
Expand All @@ -548,12 +548,12 @@ void VioManager::do_feature_propagate_update(double timestamp) {
featsup_TEMP.insert(featsup_TEMP.begin(), feats_slam_UPDATE.begin(), feats_slam_UPDATE.begin()+std::min(state->_options.max_slam_in_update,(int)feats_slam_UPDATE.size()));
feats_slam_UPDATE.erase(feats_slam_UPDATE.begin(), feats_slam_UPDATE.begin()+std::min(state->_options.max_slam_in_update,(int)feats_slam_UPDATE.size()));
// Do the update
updaterSLAM->update(state, featsup_TEMP);
updaterSLAM->update(state.get(), featsup_TEMP);
feats_slam_UPDATE_TEMP.insert(feats_slam_UPDATE_TEMP.end(), featsup_TEMP.begin(), featsup_TEMP.end());
}
feats_slam_UPDATE = feats_slam_UPDATE_TEMP;
rT5 = boost::posix_time::microsec_clock::local_time();
updaterSLAM->delayed_init(state, feats_slam_DELAYED);
updaterSLAM->delayed_init(state.get(), feats_slam_DELAYED);
rT6 = boost::posix_time::microsec_clock::local_time();


Expand Down Expand Up @@ -589,7 +589,7 @@ void VioManager::do_feature_propagate_update(double timestamp) {
//===================================================================================

// First do anchor change if we are about to lose an anchor pose
updaterSLAM->change_anchors(state);
updaterSLAM->change_anchors(state.get());

// Cleanup any features older then the marginalization time
trackFEATS->get_feature_database()->cleanup_measurements(state->margtimestep());
Expand All @@ -598,7 +598,7 @@ void VioManager::do_feature_propagate_update(double timestamp) {
}

// Finally marginalize the oldest clone if needed
StateHelper::marginalize_old_clone(state);
StateHelper::marginalize_old_clone(state.get());

// Finally if we are optimizing our intrinsics, update our trackers
if(state->_options.do_calib_camera_intrinsics) {
Expand Down
25 changes: 13 additions & 12 deletions ov_msckf/src/core/VioManager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <string>
#include <algorithm>
#include <fstream>
#include <memory>
#include <Eigen/StdVector>
#include <boost/filesystem.hpp>

Expand Down Expand Up @@ -147,22 +148,22 @@ namespace ov_msckf {

/// Accessor to get the current state
State* get_state() {
return state;
return state.get();
}

/// Accessor to get the current propagator
Propagator* get_propagator() {
return propagator;
return propagator.get();
}

/// Get feature tracker
TrackBase* get_track_feat() {
return trackFEATS;
return trackFEATS.get();
}

/// Get aruco feature tracker
TrackBase* get_track_aruco() {
return trackARUCO;
return trackARUCO.get();
}

/// Returns 3d features used in the last update in global frame
Expand Down Expand Up @@ -286,31 +287,31 @@ namespace ov_msckf {
VioManagerOptions params;

/// Our master state object :D
State* state;
std::unique_ptr<State> state;

/// Propagator of our state
Propagator* propagator;
std::unique_ptr<Propagator> propagator;

/// Our sparse feature tracker (klt or descriptor)
TrackBase* trackFEATS = nullptr;
std::unique_ptr<TrackBase> trackFEATS;

/// Our aruoc tracker
TrackBase* trackARUCO = nullptr;
std::unique_ptr<TrackBase> trackARUCO;

/// State initializer
InertialInitializer* initializer;
std::unique_ptr<InertialInitializer> initializer;

/// Boolean if we are initialized or not
bool is_initialized_vio = false;

/// Our MSCKF feature updater
UpdaterMSCKF* updaterMSCKF;
std::unique_ptr<UpdaterMSCKF> updaterMSCKF;

/// Our MSCKF feature updater
UpdaterSLAM* updaterSLAM;
std::unique_ptr<UpdaterSLAM> updaterSLAM;

/// Our aruoc tracker
UpdaterZeroVelocity* updaterZUPT = nullptr;
std::unique_ptr<UpdaterZeroVelocity> updaterZUPT;

/// Good features that where used in the last update
std::vector<Eigen::Vector3d> good_features_MSCKF;
Expand Down