Skip to content
This repository has been archived by the owner on Dec 4, 2024. It is now read-only.

Commit

Permalink
more progress
Browse files Browse the repository at this point in the history
  • Loading branch information
@manglemix
manglemix committed Mar 10, 2024
1 parent fede3c7 commit bf8805a
Show file tree
Hide file tree
Showing 4 changed files with 142 additions and 73 deletions.
3 changes: 2 additions & 1 deletion localization/Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,4 +15,5 @@ fxhash = { workspace = true }
rand_distr = { workspace = true }
rand = { workspace = true }
# eigenvalues = { workspace = true }
crossbeam = { workspace = true }
crossbeam = { workspace = true }
# ordered-float = { workspace = true }
8 changes: 6 additions & 2 deletions localization/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,7 +345,8 @@ async fn run_localizer<T: Optimizer>(

let max_delta = bb.max_delta.as_secs_f64() as Float;
new_src.point.isometry.translation.vector += 0.5
* (new_src.point.linear_velocity + (new_src.point.acceleration - G_VEC) * max_delta)
* (new_src.point.linear_velocity
+ (new_src.point.acceleration - G_VEC) * max_delta)
* max_delta;
new_src.point.linear_velocity += (new_src.point.acceleration - G_VEC) * max_delta;
new_src.point.isometry.rotation =
Expand Down Expand Up @@ -413,7 +414,10 @@ impl<T: Optimizer> Node for Localizer<T> {
* (data_point.linear_velocity + (data_point.acceleration - G_VEC) * max_delta)
* max_delta;
data_point.linear_velocity += (data_point.acceleration - G_VEC) * max_delta;
data_point.isometry.rotation = data_point.isometry.rotation.append_axisangle_linearized(&data_point.angular_velocity.scaled_axis().scale(max_delta));
data_point.isometry.rotation =
data_point.isometry.rotation.append_axisangle_linearized(
&data_point.angular_velocity.scaled_axis().scale(max_delta),
);
data_point
} else {
DataPoint {
Expand Down
200 changes: 131 additions & 69 deletions localization/src/optimizers.rs
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
use std::ops::DerefMut;
use std::{cmp::Ordering, ops::DerefMut};

use fxhash::FxHashMap;
use nalgebra::UnitQuaternion;
use rand::Rng;
use rand_distr::{Distribution, Normal};
use rig::RobotElementRef;
use unros::{
Expand Down Expand Up @@ -33,14 +32,15 @@ pub trait StochasticOptimizer: Send + Sync + 'static {
delta: Float,
imu_calibs: &FxHashMap<RobotElementRef, CalibratedImu>,
) -> DataPoint;
fn score_candidate(
fn merge_candidates(
&self,
old: &DataBucket,
current: &DataBucket,
new: &DataBucket,
candidate: &DataPoint,
candidate1: DataPoint,
candidate2: DataPoint,
imu_calibs: &FxHashMap<RobotElementRef, CalibratedImu>,
) -> usize;
) -> DataPoint;
}

impl<T: StochasticOptimizer> Optimizer for T {
Expand All @@ -52,15 +52,18 @@ impl<T: StochasticOptimizer> Optimizer for T {
delta: Float,
imu_calibs: &FxHashMap<RobotElementRef, CalibratedImu>,
) {
let Some((_, candidate)) = rayon::iter::repeatn((), self.candidate_count())
let Some(candidate) = rayon::iter::repeatn((), self.candidate_count())
.map(|_| {
let candidate = self.generate_candidate(old, current, new, delta, imu_calibs);
(
self.score_candidate(old, current, new, &candidate, imu_calibs),
candidate,
)
// let candidate = self.generate_candidate(old, current, new, delta, imu_calibs);
// let score = NotNan::new(self.score_candidate(old, current, new, &candidate, imu_calibs));
// Some((
// score.ok()?,
// candidate,
// ))
self.generate_candidate(old, current, new, delta, imu_calibs)
})
.max_by_key(|(score, _)| *score)
.reduce_with(|a, b| self.merge_candidates(old, current, new, a, b, imu_calibs))
// .max_by_key(|(score, _)| *score)
else {
return;
};
Expand Down Expand Up @@ -110,78 +113,137 @@ impl StochasticOptimizer for StochasticSmoothnessOptimizer {
.rotation_to(&new.point.isometry.rotation);
candidate.angular_velocity = UnitQuaternion::new(travel.scaled_axis().unscale(delta));

if rng.gen_bool(0.00002) {
println!("{candidate:?}");
}
// if rng.gen_bool(0.00002) {
// println!("{candidate:?}");
// }

candidate
}

fn score_candidate(
fn merge_candidates(
&self,
_old: &DataBucket,
current: &DataBucket,
_new: &DataBucket,
candidate: &DataPoint,
new: &DataBucket,
mut candidate1: DataPoint,
candidate2: DataPoint,
imu_calibs: &FxHashMap<RobotElementRef, CalibratedImu>,
) -> usize {
let mut score: Float = 0.0;

for pos_frame in &current.position_frames {
score += normal(
0.0,
pos_frame.variance.sqrt(),
(candidate.isometry.translation.vector - pos_frame.position.coords).magnitude(),
);
}
) -> DataPoint {
let get_score = |candidate: &DataPoint| {
let mut score: Float = 0.0;

for pos_frame in &current.position_frames {
score += normal(
0.0,
pos_frame.variance.sqrt(),
(candidate.isometry.translation.vector - pos_frame.position.coords).magnitude(),
);
}

for vel_frame in &current.velocity_frames {
score += normal(
0.0,
vel_frame.variance.sqrt(),
(candidate.linear_velocity - vel_frame.velocity).magnitude(),
);
}
for vel_frame in &current.velocity_frames {
score += normal(
0.0,
vel_frame.variance.sqrt(),
(candidate.linear_velocity - vel_frame.velocity).magnitude(),
);
}

for mut imu_frame in current.imu_frames.iter().cloned() {
if let Some(calibration) = imu_calibs.get(&imu_frame.robot_element) {
imu_frame.acceleration = candidate.isometry.rotation
* imu_frame.robot_element.get_isometry_from_base().rotation
* calibration.accel_correction
* imu_frame.acceleration
* calibration.accel_scale;
} else {
imu_frame.acceleration = candidate.isometry.rotation
* imu_frame.robot_element.get_isometry_from_base().rotation
* imu_frame.acceleration;
}

score += normal(
0.0,
imu_frame.acceleration_variance.sqrt(),
(candidate.acceleration - imu_frame.acceleration).magnitude(),
);
}

// if quick_rng().gen_bool(0.00002) {
// println!("{:}", current.position_frames.len());
// }
score
};

for rot_frame in &current.orientation_frames {
score += normal(
0.0,
rot_frame.variance.sqrt(),
candidate.isometry.rotation.angle_to(&rot_frame.orientation),
);
match get_score(&candidate1).total_cmp(&get_score(&candidate2)) {
Ordering::Greater => {},
Ordering::Less => {
candidate1.acceleration = candidate2.acceleration;
candidate1.linear_velocity = candidate2.linear_velocity;
candidate1.isometry.translation = candidate2.isometry.translation;
}
Ordering::Equal => {
let get_diff = |candidate: &DataPoint| {
(candidate.acceleration - new.point.acceleration).magnitude_squared() +
(candidate.linear_velocity - new.point.linear_velocity).magnitude_squared() +
(candidate.isometry.translation.vector - new.point.isometry.translation.vector).magnitude_squared()
};

if get_diff(&candidate1) > get_diff(&candidate2) {
candidate1.acceleration = candidate2.acceleration;
candidate1.linear_velocity = candidate2.linear_velocity;
candidate1.isometry.translation = candidate2.isometry.translation;
}
}
}

for mut imu_frame in current.imu_frames.iter().cloned() {
if let Some(calibration) = imu_calibs.get(&imu_frame.robot_element) {
imu_frame.angular_velocity =
calibration.angular_velocity_bias.inverse() * imu_frame.angular_velocity;
imu_frame.acceleration = candidate.isometry.rotation
* imu_frame.robot_element.get_isometry_from_base().rotation
* calibration.accel_correction
* imu_frame.acceleration
* calibration.accel_scale;
} else {
imu_frame.acceleration = candidate.isometry.rotation
* imu_frame.robot_element.get_isometry_from_base().rotation
* imu_frame.acceleration;
let get_score = |candidate: &DataPoint| {
let mut score: Float = 0.0;

for rot_frame in &current.orientation_frames {
score += normal(
0.0,
rot_frame.variance.sqrt(),
candidate.isometry.rotation.angle_to(&rot_frame.orientation),
);
}

score += normal(
0.0,
imu_frame.acceleration_variance.sqrt(),
(candidate.acceleration - imu_frame.acceleration).magnitude(),
);
score += normal(
0.0,
imu_frame.angular_velocity_variance.sqrt(),
candidate
.angular_velocity
.angle_to(&imu_frame.angular_velocity),
);
}
for mut imu_frame in current.imu_frames.iter().cloned() {
if let Some(calibration) = imu_calibs.get(&imu_frame.robot_element) {
imu_frame.angular_velocity =
calibration.angular_velocity_bias.inverse() * imu_frame.angular_velocity;
}

score += normal(
0.0,
imu_frame.angular_velocity_variance.sqrt(),
candidate
.angular_velocity
.angle_to(&imu_frame.angular_velocity),
);
}

// let mut diff: Float = 0.0;
score
};

match get_score(&candidate1).total_cmp(&get_score(&candidate2)) {
Ordering::Greater => {},
Ordering::Less => {
candidate1.angular_velocity = candidate2.angular_velocity;
candidate1.isometry.rotation = candidate2.isometry.rotation;
}
Ordering::Equal => {
let get_diff = |candidate: &DataPoint| {
candidate.angular_velocity.angle_to(&new.point.angular_velocity).powi(2) +
candidate.isometry.rotation.angle_to(&new.point.isometry.rotation).powi(2)
};

if get_diff(&candidate1) > get_diff(&candidate2) {
candidate1.angular_velocity = candidate2.angular_velocity;
candidate1.isometry.rotation = candidate2.isometry.rotation;
}
}
}

score.round() as usize
candidate1
}
}
4 changes: 3 additions & 1 deletion lunasimbot/src/main.rs
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
use costmap::Costmap;
use fxhash::FxBuildHasher;
use localization::{
frames::{IMUFrame, OrientationFrame, PositionFrame, VelocityFrame}, optimizers::StochasticSmoothnessOptimizer, Localizer
frames::{IMUFrame, OrientationFrame, PositionFrame, VelocityFrame},
optimizers::StochasticSmoothnessOptimizer,
Localizer,
};
use nalgebra::{Point3, Quaternion, UnitQuaternion, Vector3};
use navigator::{pathfinders::DirectPathfinder, DifferentialDriver};
Expand Down

0 comments on commit bf8805a

Please sign in to comment.