sophus_autodiff/dual/
dual_vector.rs

1use core::{
2    borrow::Borrow,
3    fmt::Debug,
4    ops::{
5        Add,
6        Neg,
7        Sub,
8    },
9};
10
11use approx::{
12    AbsDiffEq,
13    RelativeEq,
14};
15
16use super::{
17    dual_matrix::DualMatrix,
18    dual_scalar::DualScalar,
19    vector::{
20        HasJacobian,
21        IsDualVectorFromCurve,
22        VectorValuedDerivative,
23    },
24};
25use crate::{
26    linalg::{
27        MatF64,
28        SMat,
29        SVec,
30        VecF64,
31    },
32    prelude::*,
33};
34
35/// A dual vector, storing a set of dual scalars (with partial derivatives) for each row.
36///
37/// Conceptually, this is the forward-mode AD version of a vector in \(\mathbb{R}^\text{ROWS}\),
38/// where each element is a [`DualScalar<DM, DN>`], i.e., each element carries its own infinitesimal
39/// part.
40///
41/// # Private fields
42/// - `inner`: A`ROWS`--dimensional [`SVec`] of [`DualScalar<DM, DN>`]s.
43///
44/// # Generic Parameters
45/// - `ROWS`: Number of vector components.
46/// - `DM`, `DN`: Dimensions for each component’s derivative (infinitesimal) matrix. For example,
47///   `DM=3, DN=1` might store partial derivatives w.r.t. a 3D input for each element.
48///
49/// See [crate::dual::IsDualVector] for more details.
50#[derive(Clone, Debug, Copy)]
51pub struct DualVector<const ROWS: usize, const DM: usize, const DN: usize> {
52    /// Internal storage of the vector elements, each a `DualScalar<DM, DN>`.
53    pub(crate) inner: SVec<DualScalar<DM, DN>, ROWS>,
54}
55
56impl<const ROWS: usize, const DM: usize, const DN: usize>
57    IsDualVector<DualScalar<DM, DN>, ROWS, 1, DM, DN> for DualVector<ROWS, DM, DN>
58{
59    fn var(val: VecF64<ROWS>) -> Self {
60        let mut dij_val: SVec<DualScalar<DM, DN>, ROWS> = SVec::zeros();
61        for i in 0..ROWS {
62            dij_val[(i, 0)].real_part = val[i];
63            let mut v = SMat::<f64, DM, DN>::zeros();
64            // identity derivative for the i-th element w.r.t. that row
65            v[(i, 0)] = 1.0;
66            dij_val[(i, 0)].infinitesimal_part = Some(v);
67        }
68        Self { inner: dij_val }
69    }
70
71    fn derivative(&self) -> VectorValuedDerivative<f64, ROWS, 1, DM, DN> {
72        let mut v = SVec::<SMat<f64, DM, DN>, ROWS>::zeros();
73        for i in 0..ROWS {
74            v[i] = self.inner[i].derivative();
75        }
76        VectorValuedDerivative { out_vec: v }
77    }
78}
79
80impl<const ROWS: usize> IsDualVectorFromCurve<DualScalar<1, 1>, ROWS, 1>
81    for DualVector<ROWS, 1, 1>
82{
83    fn curve_derivative(&self) -> VecF64<ROWS> {
84        self.jacobian()
85    }
86}
87
88impl<const ROWS: usize, const DM: usize> HasJacobian<DualScalar<DM, 1>, ROWS, 1, DM>
89    for DualVector<ROWS, DM, 1>
90{
91    fn jacobian(&self) -> MatF64<ROWS, DM> {
92        let mut v = SMat::<f64, ROWS, DM>::zeros();
93        for i in 0..ROWS {
94            let d = self.inner[i].derivative();
95            // each element’s derivative is DM x 1 => we collect the column into row i
96            for j in 0..DM {
97                v[(i, j)] = d[j];
98            }
99        }
100        v
101    }
102}
103
104impl<const ROWS: usize, const DM: usize, const DN: usize> num_traits::Zero
105    for DualVector<ROWS, DM, DN>
106{
107    fn zero() -> Self {
108        DualVector {
109            inner: SVec::zeros(),
110        }
111    }
112
113    fn is_zero(&self) -> bool {
114        self.inner == Self::zero().inner
115    }
116}
117
118impl<const ROWS: usize, const DM: usize, const DN: usize>
119    IsSingleVector<DualScalar<DM, DN>, ROWS, DM, DN> for DualVector<ROWS, DM, DN>
120where
121    DualVector<ROWS, DM, DN>: IsVector<DualScalar<DM, DN>, ROWS, 1, DM, DN>,
122{
123    fn set_real_scalar(&mut self, idx: usize, v: f64) {
124        self.inner[idx].real_part = v;
125    }
126}
127
128impl<const ROWS: usize, const DM: usize, const DN: usize> Neg for DualVector<ROWS, DM, DN> {
129    type Output = DualVector<ROWS, DM, DN>;
130
131    fn neg(self) -> Self::Output {
132        DualVector { inner: -self.inner }
133    }
134}
135
136impl<const ROWS: usize, const DM: usize, const DN: usize> Sub for DualVector<ROWS, DM, DN> {
137    type Output = DualVector<ROWS, DM, DN>;
138
139    fn sub(self, rhs: Self) -> Self::Output {
140        DualVector {
141            inner: self.inner - rhs.inner,
142        }
143    }
144}
145
146impl<const ROWS: usize, const DM: usize, const DN: usize> Add for DualVector<ROWS, DM, DN> {
147    type Output = DualVector<ROWS, DM, DN>;
148
149    fn add(self, rhs: Self) -> Self::Output {
150        DualVector {
151            inner: self.inner + rhs.inner,
152        }
153    }
154}
155
156impl<const ROWS: usize, const DM: usize, const DN: usize> PartialEq for DualVector<ROWS, DM, DN> {
157    fn eq(&self, other: &Self) -> bool {
158        self.inner == other.inner
159    }
160}
161
162impl<const ROWS: usize, const DM: usize, const DN: usize> AbsDiffEq for DualVector<ROWS, DM, DN> {
163    type Epsilon = f64;
164
165    fn default_epsilon() -> Self::Epsilon {
166        f64::default_epsilon()
167    }
168
169    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
170        self.inner.abs_diff_eq(&other.inner, epsilon)
171    }
172}
173
174impl<const ROWS: usize, const DM: usize, const DN: usize> RelativeEq for DualVector<ROWS, DM, DN> {
175    fn default_max_relative() -> Self::Epsilon {
176        f64::default_max_relative()
177    }
178
179    fn relative_eq(
180        &self,
181        other: &Self,
182        epsilon: Self::Epsilon,
183        max_relative: Self::Epsilon,
184    ) -> bool {
185        self.inner.relative_eq(&other.inner, epsilon, max_relative)
186    }
187}
188
189impl<const ROWS: usize, const DM: usize, const DN: usize>
190    IsVector<DualScalar<DM, DN>, ROWS, 1, DM, DN> for DualVector<ROWS, DM, DN>
191{
192    fn from_f64(val: f64) -> Self {
193        DualVector {
194            inner: SVec::<DualScalar<DM, DN>, ROWS>::from_element(DualScalar::from_f64(val)),
195        }
196    }
197
198    fn norm(&self) -> DualScalar<DM, DN> {
199        // accumulate dot = x^T x, then sqrt
200        self.dot(*self).sqrt()
201    }
202
203    fn squared_norm(&self) -> DualScalar<DM, DN> {
204        self.dot(*self)
205    }
206
207    fn elem(&self, idx: usize) -> DualScalar<DM, DN> {
208        self.inner[idx]
209    }
210
211    fn from_array<A>(duals: A) -> Self
212    where
213        A: Borrow<[DualScalar<DM, DN>; ROWS]>,
214    {
215        DualVector {
216            inner: SVec::<DualScalar<DM, DN>, ROWS>::from_row_slice(&duals.borrow()[..]),
217        }
218    }
219
220    fn from_real_array<A>(vals: A) -> Self
221    where
222        A: Borrow<[f64; ROWS]>,
223    {
224        let vals = vals.borrow();
225        let mut out = Self::zeros();
226        for i in 0..ROWS {
227            out.inner[i].real_part = vals[i];
228        }
229        out
230    }
231
232    fn from_real_vector<A>(val: A) -> Self
233    where
234        A: Borrow<VecF64<ROWS>>,
235    {
236        let vals = val.borrow();
237        let mut out = Self::zeros();
238        for i in 0..ROWS {
239            out.inner[i].real_part = vals[i];
240        }
241        out
242    }
243
244    fn real_vector(&self) -> VecF64<ROWS> {
245        let mut r = VecF64::<ROWS>::zeros();
246        for i in 0..ROWS {
247            r[i] = self.elem(i).real_part;
248        }
249        r
250    }
251
252    fn to_mat(&self) -> DualMatrix<ROWS, 1, DM, DN> {
253        DualMatrix { inner: self.inner }
254    }
255
256    fn block_vec2<const R0: usize, const R1: usize>(
257        top_row: DualVector<R0, DM, DN>,
258        bot_row: DualVector<R1, DM, DN>,
259    ) -> Self {
260        assert_eq!(R0 + R1, ROWS);
261        let mut m = Self::zeros();
262        m.inner
263            .fixed_view_mut::<R0, 1>(0, 0)
264            .copy_from(&top_row.inner);
265        m.inner
266            .fixed_view_mut::<R1, 1>(R0, 0)
267            .copy_from(&bot_row.inner);
268        m
269    }
270
271    fn block_vec3<const R0: usize, const R1: usize, const R2: usize>(
272        top_row: DualVector<R0, DM, DN>,
273        middle_row: DualVector<R1, DM, DN>,
274        bot_row: DualVector<R2, DM, DN>,
275    ) -> Self {
276        assert_eq!(R0 + R1 + R2, ROWS);
277        let mut m = Self::zeros();
278        m.inner
279            .fixed_view_mut::<R0, 1>(0, 0)
280            .copy_from(&top_row.inner);
281        m.inner
282            .fixed_view_mut::<R1, 1>(R0, 0)
283            .copy_from(&middle_row.inner);
284        m.inner
285            .fixed_view_mut::<R2, 1>(R1, 0)
286            .copy_from(&bot_row.inner);
287        m
288    }
289
290    fn scaled(&self, s: DualScalar<DM, DN>) -> Self {
291        // scale each element by s
292        Self {
293            inner: self.inner * *s.borrow(),
294        }
295    }
296
297    fn dot<V>(&self, rhs: V) -> DualScalar<DM, DN>
298    where
299        V: Borrow<Self>,
300    {
301        // sum_{i} self[i] * rhs[i]
302        let mut sum = DualScalar::<DM, DN>::from_f64(0.0);
303        for i in 0..ROWS {
304            sum += self.elem(i) * rhs.borrow().elem(i);
305        }
306        sum
307    }
308
309    fn normalized(&self) -> Self {
310        self.scaled(DualScalar::<DM, DN>::from_f64(1.0) / self.norm())
311    }
312
313    fn from_f64_array<A>(vals: A) -> Self
314    where
315        A: Borrow<[f64; ROWS]>,
316    {
317        let vals = vals.borrow();
318        let mut out = Self::zeros();
319        for i in 0..ROWS {
320            out.inner[i].real_part = vals[i];
321        }
322        out
323    }
324
325    fn from_scalar_array<A>(vals: A) -> Self
326    where
327        A: Borrow<[DualScalar<DM, DN>; ROWS]>,
328    {
329        DualVector {
330            inner: SVec::<DualScalar<DM, DN>, ROWS>::from_row_slice(&vals.borrow()[..]),
331        }
332    }
333
334    fn elem_mut(&mut self, idx: usize) -> &mut DualScalar<DM, DN> {
335        &mut self.inner[idx]
336    }
337
338    fn to_dual_const<const M: usize, const N: usize>(
339        &self,
340    ) -> <DualScalar<DM, DN> as IsScalar<1, DM, DN>>::DualVector<ROWS, M, N> {
341        // Discard derivative part, just copy real parts.
342        DualVector::<ROWS, M, N>::from_real_vector(self.real_vector())
343    }
344
345    fn outer<const R2: usize, V>(
346        &self,
347        rhs: V,
348    ) -> <DualScalar<DM, DN> as IsScalar<1, DM, DN>>::Matrix<ROWS, R2>
349    where
350        V: Borrow<DualVector<R2, DM, DN>>,
351    {
352        // build a dual matrix with each entry = self[i] * rhs[j]
353        let mut out = DualMatrix::<ROWS, R2, DM, DN>::zeros();
354        for i in 0..ROWS {
355            for j in 0..R2 {
356                *out.elem_mut([i, j]) = self.elem(i) * rhs.borrow().elem(j);
357            }
358        }
359        out
360    }
361
362    fn select<Q>(&self, mask: &bool, other: Q) -> Self
363    where
364        Q: Borrow<Self>,
365    {
366        // single-lane bool => entire vector is chosen or not
367        if *mask { *self } else { *other.borrow() }
368    }
369
370    fn get_fixed_subvec<const R: usize>(&self, start_r: usize) -> DualVector<R, DM, DN> {
371        // sub-slice from row start_r to row start_r+R
372        DualVector {
373            inner: self.inner.fixed_rows::<R>(start_r).into(),
374        }
375    }
376}
sophus_autodiff/dual/dual_vector.rs

sophus_autodiff/dual/
dual_vector.rs
