Sophus + ceres 对SE3的位姿优化实践,PnP
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  6.971092e+01    0.00e+00    2.29e+01   0.00e+00   0.00e+00  1.00e+04        0    7.45e-05    2.28e-04
   1  5.852248e+01    1.12e+01    8.91e+00   1.03e+01   1.82e+00  3.00e+04        0    1.41e-04    4.02e-04
   2  2.764596e+01    3.09e+01    2.00e+01   2.52e+00   4.94e+00  9.00e+04        0    7.64e-05    4.94e-04
   3  7.834946e-02    2.76e+01    1.37e+01   2.36e-01   4.48e+00  2.70e+05        0    7.05e-05    5.76e-04
   4  4.140774e-02    3.69e-02    1.03e+01   2.52e-03   1.09e+00  8.10e+05        0    6.93e-05    6.57e-04
   5  4.119084e-02    2.17e-04    1.67e+01   2.11e-04   1.07e+00  2.43e+06        0    6.94e-05    7.40e-04
   6  4.118984e-02    1.00e-06    1.63e+00   1.41e-05   1.06e+00  7.29e+06        0    6.83e-05    8.18e-04
   7  4.118983e-02    4.19e-09    1.06e-01   9.14e-07   1.06e+00  2.19e+07        0    6.74e-05    8.94e-04
Ceres Solver Report: Iterations: 8, Initial cost: 6.971092e+01, Final cost: 4.118983e-02, Termination: CONVERGENCE
after optimize:
 0.925421 -0.287927  0.246363    -0.722
 0.336739   0.92301  -0.18617  -8.15989
-0.173792  0.255245  0.951129   9.10729
        0         0         0         1
err_opt:0.0038567      -0.00193814   0.00258262   -0.0020643 -0.000398187 -0.000119575  0.000116657
true value:
 0.925417 -0.288133  0.246137 -0.725046
 0.336824  0.923045 -0.185843  -8.15777
-0.173648  0.254887  0.951251   9.10632
        0         0         0         1
err_init:10.3912      -3.06362   8.02512  -5.81668  -0.14186  0.155837 -0.557138
                                    
                                    
                                        
                                            1