readme

Author: powei-lin

README.md

@@ -42,6 +42,7 @@ cargo add tiny-solver
 It's not extremely optimized, but it's easy to install and use.
 
 ## Usage
+Rust 
 ```rust
 // define your own Cost/Factor struct
 // impl residual function
@@ -103,6 +104,55 @@ fn main() {
     }
 }
 ```
+Python
+```py
+import numpy as np
+from tiny_solver import Problem, GaussNewtonOptimizer
+from tiny_solver.factors import PriorFactor, PyFactor
+
+# define custom cost function in python
+def cost(x: np.ndarray, yz: np.ndarray) -> np.ndarray:
+    r0 = x[0] + 2 * yz[0] + 4 * yz[1]
+    r1 = yz[0] * yz[0]
+    return np.array([r0, r1])
+
+
+def main():
+
+    # initialize problem (factor graph)
+    problem = Problem()
+
+    # factor defined in python
+    custom_factor = PyFactor(cost)
+    problem.add_residual_block(
+        2,
+        [
+            ("x", 1),
+            ("yz", 2),
+        ],
+        custom_factor,
+        None,
+    )
+
+    # prior factor import from rust
+    prior_factor = PriorFactor(np.array([3.0]))
+    problem.add_residual_block(1, [("x", 1)], prior_factor, None)
+
+    # initial values
+    init_values = {"x": np.array([0.7]), "yz": np.array([-30.2, 123.4])}
+
+    # optimizer
+    optimizer = GaussNewtonOptimizer()
+    result_values = optimizer.optimize(problem, init_values)
+
+    # result
+    for k, v in result_values.items():
+        print(f"{k}: {v}")
+
+
+if __name__ == "__main__":
+    main()
+```
 
 ## Example
 ### Basic example

examples/python/m3500.py

@@ -51,7 +51,7 @@ def show_pose(init_values, color):
 
 
 def main():
-    FORMAT = '%(levelname)s %(name)s %(asctime)-15s %(filename)s:%(lineno)d %(message)s'
+    FORMAT = "%(levelname)s %(name)s %(asctime)-15s %(filename)s:%(lineno)d %(message)s"
     logging.basicConfig(format=FORMAT)
     logging.getLogger().setLevel(logging.INFO)
     file_path = "tests/data/input_M3500_g2o.g2o"

examples/python/small_problem.py

@@ -2,32 +2,44 @@
 from tiny_solver import Problem, GaussNewtonOptimizer
 from tiny_solver.factors import PriorFactor, PyFactor
 
-
-def cost(x, y, z):
-    r0 = x[0] + 2 * y[0] + 4 * z[0]
-    r1 = y[0] * z[0]
+# define custom cost function in python
+def cost(x: np.ndarray, yz: np.ndarray) -> np.ndarray:
+    r0 = x[0] + 2 * yz[0] + 4 * yz[1]
+    r1 = yz[0] * yz[0]
     return np.array([r0, r1])
 
 
 def main():
+
+    # initialize problem (factor graph)
     problem = Problem()
-    pf = PyFactor(cost)
+
+    # factor defined in python
+    custom_factor = PyFactor(cost)
     problem.add_residual_block(
         2,
         [
             ("x", 1),
-            ("y", 1),
-            ("z", 1),
+            ("yz", 2),
         ],
-        pf,
+        custom_factor,
         None,
     )
-    pp = PriorFactor(np.array([3.0]))
-    problem.add_residual_block(1, [("x", 1)], pp, None)
-    gn = GaussNewtonOptimizer()
-    init_values = {"x": np.array([0.7]), "y": np.array([-30.2]), "z": np.array([123.9])}
-    result_values = gn.optimize(problem, init_values)
-    print(result_values)
+
+    # prior factor import from rust
+    prior_factor = PriorFactor(np.array([3.0]))
+    problem.add_residual_block(1, [("x", 1)], prior_factor, None)
+
+    # initial values
+    init_values = {"x": np.array([0.7]), "yz": np.array([-30.2, 123.4])}
+
+    # optimizer
+    optimizer = GaussNewtonOptimizer()
+    result_values = optimizer.optimize(problem, init_values)
+
+    # result
+    for k, v in result_values.items():
+        print(f"{k}: {v}")
 
 
 if __name__ == "__main__":