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; }