void getcamPose(cv::Point3f & campose)
{
cv::Mat invR(3,3,CV_64F);
invR=rotateMat.t();
cv::Mat camPos(3,1,CV_64F); //正交阵求逆
camPos=invR*tranVector;
// std::cout<<"invR="<<invR<<std::endl;
//std::cout<<"camPos"<<camPos<<std::endl;
campose.x=-1*camPos.at<double>(0,0);
campose.y=-1*camPos.at<double>(1,0);
campose.z=-1*camPos.at<double>(2,0);
}
/// \brief evaluate the error term and return the weighted squared error e^T invR e
double ErrorTermTransformation::evaluateErrorImplementation()
{
_errorMatrix = _T.toTransformationMatrix()*_prior.inverse().T();
Eigen::VectorXd errorVector = Eigen::VectorXd::Zero(6);
errorVector.block(0,0,3,1) = _errorMatrix.block(0,3,3,1);
errorVector.block(3,0,3,1) = sm::kinematics::R2AxisAngle(_errorMatrix.block(0,0,3,3));
setError(errorVector);
if (_debug == 1)
{
std::cout << "\n\n\nPriorEstim:\n" << _T.toTransformationMatrix() << "\n";
std::cout << "PriorGiven:\n" << _prior.T() << "\n";
std::cout << "invPrior:\n" << _prior.inverse().T() << "\n";
std::cout << "errorMatrix:\n" << _errorMatrix << "\n";
std::cout << "error:\n" << error() << "\ninvR:\n" << invR() << "\n\n\n";
}
return evaluateChiSquaredError();
}
