#1: Try to update kiss3d dependency in order to get rid of panic

Cargo.toml

@@ -31,11 +31,11 @@ coveralls = { repository = "TNG/gs-rs", branch = "master", service = "github" }
 maintenance = { status = "actively-developed" }
 
 [dependencies]
-nalgebra = { version = "0.22.0", features = ["sparse"] }
+nalgebra = { version = "0.26.1", features = ["sparse"] }
 serde = { version = "1.0.115", features = ["derive"] }
 serde_json = "1.0.57"
 petgraph = "0.5.1"
-kiss3d = "0.25.0"
+kiss3d = "0.31.0"
 itertools = "0.9.0"
 
 [dev-dependencies]

src/optimizer/linear_system/iso3d_gradients.rs

@@ -11,6 +11,7 @@
 //
 
 use nalgebra::{Isometry3, Matrix3, MatrixMN, Quaternion, Translation3, UnitQuaternion, U3, U9};
+use nalgebra::storage::Storage;
 
 // code copied from g2o for the case [ sin >= 0 && trace > 0 ]
 pub fn calc_dq_dR(matr: &Matrix3<f64>) -> MatrixMN<f64, U3, U9> {

src/optimizer/linear_system/obs2d_handler.rs

@@ -19,6 +19,7 @@ use nalgebra::{
     DMatrix, DVector, Dynamic, Matrix, Matrix2x5, Matrix5x2, Rotation2, RowVector2, SliceStorage, Vector, Vector2, U1,
     U5,
 };
+use nalgebra::storage::Storage;
 
 pub fn update_H_b(
     H: &mut DMatrix<f64>,
@@ -39,8 +40,8 @@ pub fn update_H_b(
     update_H_submatrix(H, &H_updates.index((3.., ..3)), &var_j.fixed_type, &var_i.fixed_type);
     update_H_submatrix(H, &H_updates.index((3.., 3..)), &var_j.fixed_type, &var_j.fixed_type);
 
-    let err_pos = Rotation2::new(-rot_i) * (pos_j - pos_i) - pos_ij;
-    let err_vec = err_pos.data.to_vec();
+    let err_pos: Vector2<f64> = Rotation2::new(-rot_i) * (pos_j - pos_i) - pos_ij;
+    let err_vec = err_pos.data.as_slice().to_vec();
     let b_updates = (RowVector2::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_updates.index((..3, ..)), &var_i.fixed_type);
     update_b_subvector(b, &b_updates.index((3.., ..)), &var_j.fixed_type);

src/optimizer/linear_system/obs3d_handler.rs

@@ -20,6 +20,8 @@ use nalgebra::{
     DMatrix, DVector, Dynamic, Isometry3, Matrix, Matrix3, MatrixMN, RowVector3, SliceStorage, Translation3, Vector,
     Vector3, U1, U3, U9,
 };
+use nalgebra::storage::Storage;
+
 
 pub fn update_H_b(
     H: &mut DMatrix<f64>,
@@ -42,7 +44,7 @@ pub fn update_H_b(
     update_H_submatrix(H, &H_updates.index((6.., 6..)), &var_j.fixed_type, &var_j.fixed_type);
 
     let err_pos = local_j.vector - pos_ij;
-    let err_vec = err_pos.data.to_vec();
+    let err_vec = err_pos.data.as_slice().to_vec();
     let b_updates = (RowVector3::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_updates.index((..6, ..)), &var_i.fixed_type);
     update_b_subvector(b, &b_updates.index((6.., ..)), &var_j.fixed_type);

src/optimizer/linear_system/odo2d_handler.rs

@@ -20,6 +20,7 @@ use nalgebra::{
     Vector, Vector2, Vector3, U1, U6,
 };
 use std::f64::consts::PI;
+use nalgebra::storage::Storage;
 
 pub fn update_H_b(
     H: &mut DMatrix<f64>,
@@ -47,7 +48,7 @@ pub fn update_H_b(
     } else if err_rot < -PI {
         err_rot += 2.0 * PI;
     }
-    let mut err_vec = err_pos.data.to_vec();
+    let mut err_vec = err_pos.data.as_slice().to_vec();
     err_vec.push(err_rot);
     let b_updates = (RowVector3::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_updates.index((..3, ..)), &var_i.fixed_type);

src/optimizer/linear_system/odo3d_handler.rs

@@ -22,6 +22,7 @@ use nalgebra::{
     DMatrix, DVector, Dynamic, Isometry3, Matrix, Matrix3, Matrix6, MatrixMN, RowVector6, SliceStorage, Vector, U1,
     U12, U6,
 };
+use nalgebra::storage::Storage;
 
 pub fn update_H_b(
     H: &mut DMatrix<f64>,
@@ -43,8 +44,8 @@ pub fn update_H_b(
     update_H_submatrix(H, &H_updates.index((6.., 6..)), &var_j.fixed_type, &var_j.fixed_type);
 
     let err = iso_ij.inverse() * iso_i.inverse() * iso_j;
-    let mut err_vec = err.translation.vector.data.to_vec();
-    err_vec.extend_from_slice(&err.rotation.quaternion().coords.data.to_vec()[..3]);
+    let mut err_vec = err.translation.vector.data.as_slice().to_vec();
+    err_vec.extend_from_slice(&err.rotation.quaternion().coords.data.as_slice().to_vec()[..3]);
     let b_updates = (RowVector6::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_updates.index((..6, ..)), &var_i.fixed_type);
     update_b_subvector(b, &b_updates.index((6.., ..)), &var_j.fixed_type);

src/optimizer/linear_system/pos2d_handler.rs

@@ -19,6 +19,8 @@ use nalgebra::{
     ArrayStorage, DMatrix, DVector, Matrix, Matrix3, Rotation2, Rotation3, RowVector3, Vector, Vector2, Vector3, U1, U3,
 };
 use std::{f64::consts::PI, ops::Range};
+use nalgebra::storage::Storage;
+
 
 pub fn update_H_b(H: &mut DMatrix<f64>, b: &mut DVector<f64>, factor: &Factor, var: &VehicleVariable2D) {
     let range = if let FixedType::NonFixed(range) = &var.fixed_type {
@@ -42,7 +44,7 @@ pub fn update_H_b(H: &mut DMatrix<f64>, b: &mut DVector<f64>, factor: &Factor, v
     } else if err_rot < -PI {
         err_rot += 2.0 * PI;
     }
-    let mut err_vec = err_pos.data.to_vec();
+    let mut err_vec = err_pos.data.as_slice().to_vec();
     err_vec.push(err_rot);
     let b_update = (RowVector3::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_update, range.to_owned());
@@ -55,7 +57,7 @@ fn calc_jacobians(rot_m: f64) -> (Matrix3<f64>, Matrix3<f64>) {
 
 fn update_H_submatrix(
     H: &mut DMatrix<f64>,
-    added_matrix: &Matrix<f64, U3, U3, ArrayStorage<f64, U3, U3>>,
+    added_matrix: &Matrix<f64, U3, U3, ArrayStorage<f64, { 3 }, { 3 }>>,
     range: Range<usize>,
 ) {
     let updated_submatrix = &(H.index((range.clone(), range.clone())) + added_matrix);
@@ -64,7 +66,7 @@ fn update_H_submatrix(
 
 fn update_b_subvector(
     b: &mut DVector<f64>,
-    added_vector: &Vector<f64, U3, ArrayStorage<f64, U3, U1>>,
+    added_vector: &Vector<f64, U3, ArrayStorage<f64, { 3 }, { 1 } >>,
     range: Range<usize>,
 ) {
     let updated_subvector = &(b.index((range.clone(), ..)) + added_vector);

src/optimizer/linear_system/pos3d_handler.rs

@@ -18,6 +18,7 @@ use crate::factor_graph::variable::{FixedType, VehicleVariable3D};
 use crate::optimizer::linear_system::iso3d_gradients::{calc_dq_dR, get_isometry, skew_matr_and_mult_parts};
 use nalgebra::{ArrayStorage, DMatrix, DVector, Isometry3, Matrix, Matrix3, Matrix6, RowVector6, Vector, U1, U6};
 use std::ops::Range;
+use nalgebra::storage::Storage;
 
 pub fn update_H_b(H: &mut DMatrix<f64>, b: &mut DVector<f64>, factor: &Factor, var: &VehicleVariable3D) {
     let range = if let FixedType::NonFixed(range) = &var.fixed_type {
@@ -35,8 +36,8 @@ pub fn update_H_b(H: &mut DMatrix<f64>, b: &mut DVector<f64>, factor: &Factor, v
     update_H_submatrix(H, &H_update, &range);
 
     let err = iso_m.inverse() * iso_v;
-    let mut err_vec = err.translation.vector.data.to_vec();
-    err_vec.extend_from_slice(&err.rotation.quaternion().coords.data.to_vec()[..3]);
+    let mut err_vec = err.translation.vector.data.as_slice().to_vec();
+    err_vec.extend_from_slice(&err.rotation.quaternion().coords.data.as_slice().to_vec()[..3]);
     let b_update = (RowVector6::from_vec(err_vec) * &right_mult).transpose();
     update_b_subvector(b, &b_update, &range);
 }
@@ -57,7 +58,7 @@ fn calc_jacobians(iso_v: &Isometry3<f64>, iso_m: &Isometry3<f64>) -> (Matrix6<f6
 
 fn update_H_submatrix(
     H: &mut DMatrix<f64>,
-    added_matrix: &Matrix<f64, U6, U6, ArrayStorage<f64, U6, U6>>,
+    added_matrix: &Matrix<f64, U6, U6, ArrayStorage<f64, { 6 }, { 6 }>>,
     range: &Range<usize>,
 ) {
     let updated_submatrix = &(H.index((range.to_owned(), range.to_owned())) + added_matrix);
@@ -67,7 +68,7 @@ fn update_H_submatrix(
 
 fn update_b_subvector(
     b: &mut DVector<f64>,
-    added_vector: &Vector<f64, U6, ArrayStorage<f64, U6, U1>>,
+    added_vector: &Vector<f64, U6, ArrayStorage<f64, { 6 }, { 1 }>>,
     range: &Range<usize>,
 ) {
     let updated_subvector = &(b.index((range.to_owned(), ..)) + added_vector);

src/optimizer/mod.rs

@@ -21,6 +21,7 @@ use crate::optimizer::linear_system::iso3d_gradients::{get_isometry, get_isometr
 use crate::optimizer::solver::sparse_cholesky::SparseCholeskySolver;
 use crate::optimizer::solver::Solver;
 use std::f64::consts::PI;
+use nalgebra::storage::Storage;
 
 mod linear_system;
 mod solver;
@@ -78,8 +79,8 @@ fn update_var(var: &Variable, solution: &[f64]) {
             let old_iso = get_isometry(&*var.pose.borrow());
             let cor_iso = get_isometry_normalized(correction);
             let new_iso = old_iso * cor_iso;
-            let mut updated_content = new_iso.translation.vector.data.to_vec();
-            updated_content.extend(&new_iso.rotation.quaternion().coords.data.to_vec());
+            let mut updated_content = new_iso.translation.vector.data.as_slice().to_vec();
+            updated_content.extend(&new_iso.rotation.quaternion().coords.data.as_slice().to_vec());
             updated_content
         }
         Variable::Landmark3D(var) => var