| 459 | } |
| 460 | |
| 461 | void Sim3Solver::Project(const vector<Eigen::Vector3f> &vP3Dw, vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw, GeometricCamera* pCamera) |
| 462 | { |
| 463 | Eigen::Matrix3f Rcw = Tcw.block<3,3>(0,0); |
| 464 | Eigen::Vector3f tcw = Tcw.block<3,1>(0,3); |
| 465 | |
| 466 | vP2D.clear(); |
| 467 | vP2D.reserve(vP3Dw.size()); |
| 468 | |
| 469 | for(size_t i=0, iend=vP3Dw.size(); i<iend; i++) |
| 470 | { |
| 471 | Eigen::Vector3f P3Dc = Rcw*vP3Dw[i]+tcw; |
| 472 | Eigen::Vector2f pt2D = pCamera->project(P3Dc); |
| 473 | vP2D.push_back(pt2D); |
| 474 | } |
| 475 | } |
| 476 | |
| 477 | void Sim3Solver::FromCameraToImage(const vector<Eigen::Vector3f> &vP3Dc, vector<Eigen::Vector2f> &vP2D, GeometricCamera* pCamera) |
| 478 | { |