| 44 | } |
| 45 | |
| 46 | auto PointToPlaneAligningTransform::calculateAmendment() const -> RigidScaleXf3d |
| 47 | { |
| 48 | assert( sumAIsSym_ ); |
| 49 | Eigen::LLT<Eigen::MatrixXd> chol( sumA_.topLeftCorner<6,6>() ); |
| 50 | Eigen::VectorXd solution = chol.solve( sumB_.topRows<6>() - sumA_.block<6,1>( 0, 6 ) ); |
| 51 | |
| 52 | RigidScaleXf3d res; |
| 53 | res.a = Vector3d{ solution.coeff( 0 ), solution.coeff( 1 ), solution.coeff( 2 ) }; |
| 54 | res.b = Vector3d{ solution.coeff( 3 ), solution.coeff( 4 ), solution.coeff( 5 ) }; |
| 55 | return res; |
| 56 | } |
| 57 | |
| 58 | auto PointToPlaneAligningTransform::calculateAmendmentWithScale() const -> RigidScaleXf3d |
| 59 | { |