1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

use pyo3::prelude::*;

use crate::factors::*;
use crate::problem::Problem;

fn convert_pyany_to_factor(py_any: &PyAny) -> PyResult<Box<dyn Factor + Send>> {
    let factor_name: String = py_any.get_type().getattr("__name__")?.extract()?;
    match factor_name.as_str() {
        "CostFactorSE2" => {
            let factor: CostFactorSE2 = py_any.extract().unwrap();
            Ok(Box::new(factor))
        }
        "PriorFactor" => {
            let factor: PriorFactor = py_any.extract().unwrap();
            Ok(Box::new(factor))
        }
        _ => Err(PyErr::new::<pyo3::exceptions::PyValueError, _>(
            "Unknown factor type",
        )),
    }
}

#[pymethods]
impl Problem {
    #[new]
    pub fn new_py() -> Self {
        Problem::new()
    }

    #[pyo3(name = "add_residual_block")]
    pub fn add_residual_block_py(
        &mut self,
        dim_residual: usize,
        variable_key_size_list: Vec<(String, usize)>,
        pyfactor: &PyAny,
    ) -> PyResult<()> {
        self.add_residual_block(
            dim_residual,
            variable_key_size_list,
            convert_pyany_to_factor(pyfactor).unwrap(),
        );

        // println!(
        //     "total residual {}, total keys {}",
        //     self.total_residual_dimension,
        //     self.variable_name_to_col_idx_dict.len()
        // );
        Ok(())
    }

    #[getter]
    pub fn get_num(&self) -> PyResult<usize> {
        Ok(self.thread_num)
    }
    #[setter]
    pub fn set_num(&mut self, value: usize) -> PyResult<()> {
        self.thread_num = value;
        Ok(())
    }
}