void cxy_icp_kinematic_chain<_Scalar>::getJacobian(MatrixXX& jacobian)
{
long rows = cxy_config::n_num_*getModelPointSize();
long cols = cxy_config::joint_DoFs;
if (rows != jacobian.rows()
|| cols != jacobian.cols())
{
jacobian.resize(rows, cols);
}
jacobian.setZero();
int row = 0;
for (int ii = 0; ii < points_.size(); ++ii)
{
row = ii*config_->n_num_;
//jacobian.row(1);
points_[ii]->computePointJacobian(jacobian.block(row, 0, cxy_config::n_num_, jacobian.cols()));
}
fout_res_<<"x = "<<std::endl;
fout_res_<<Rad2Deg(x_)<<std::endl;
fout_res_<<"jac = "<<std::endl;
fout_res_<<jacobian<<std::endl;
}
