void doArcball(
double * _viewMatrix,
double const * _rotationCenter,
double const * _projectionMatrix,
double const * _initialViewMatrix,
//double const * _currentViewMatrix,
double const * _initialMouse,
double const * _currentMouse,
bool screenSpaceRadius,
double radius)
{
Eigen::Map<Eigen::Vector3d const> rotationCenter(_rotationCenter);
Eigen::Map<Eigen::Matrix4d const> initialViewMatrix(_initialViewMatrix);
//Eigen::Map<Eigen::Matrix4d const> currentViewMatrix(_currentViewMatrix);
Eigen::Map<Eigen::Matrix4d const> projectionMatrix(_projectionMatrix);
Eigen::Map<Eigen::Matrix4d> viewMatrix(_viewMatrix);
Eigen::Vector2d initialMouse(_initialMouse);
Eigen::Vector2d currentMouse(_currentMouse);
Eigen::Quaterniond q;
Eigen::Vector3d Pa, Pc;
if (screenSpaceRadius) {
double aspectRatio = projectionMatrix(1, 1) / projectionMatrix(0, 0);
initialMouse(0) *= aspectRatio;
currentMouse(0) *= aspectRatio;
Pa = mapToSphere(initialMouse, radius);
Pc = mapToSphere(currentMouse, radius);
q.setFromTwoVectors(Pa - rotationCenter, Pc - rotationCenter);
}
else {
Pa = mapToSphere(projectionMatrix.inverse(), initialMouse, rotationCenter, radius);
Pc = mapToSphere(projectionMatrix.inverse(), currentMouse, rotationCenter, radius);
Eigen::Vector3d a = Pa - rotationCenter, b = Pc - rotationCenter;
#if 0
std::cout << "Mouse Initial Radius = " << sqrt(a.dot(a)) << " Current Radius = " << sqrt(b.dot(b)) << std::endl;
#endif
q.setFromTwoVectors(a, b);
}
Eigen::Matrix4d qMat4;
qMat4.setIdentity();
qMat4.topLeftCorner<3, 3>() = q.toRotationMatrix();
Eigen::Matrix4d translate;
translate.setIdentity();
translate.topRightCorner<3, 1>() = -rotationCenter;
Eigen::Matrix4d invTranslate;
invTranslate.setIdentity();
invTranslate.topRightCorner<3, 1>() = rotationCenter;
viewMatrix = invTranslate * qMat4 * translate * initialViewMatrix;
}
//debug
void Dataset_analyzer::testetransformation(std::string file_pose_s, std::string file_pose_d, std::string ply_filename_s,std::string ply_filename_d){
PCloud cloud_s;
PCloud cloud_d;
LoadCloudFromTXT(file_pose_s.data(), cloud_s);
LoadCloudFromTXT(file_pose_d.data(), cloud_d);
Eigen::Matrix4d Ts; Ts.setIdentity();
Ts.block<3,3>(0,0) = (qs*qp).toRotationMatrix ();
Ts.block<3,1>(0,3) = ts;
Eigen::Matrix4d T = Computetransformation();
//T.setIdentity();
//T.block<3,3>(0,0) = (qs*qp).toRotationMatrix()*(qd*qp).toRotationMatrix().transpose();
//T.block<3,1>(0,3) = td-ts;
Eigen::Matrix4d Taux; Taux.setIdentity();
Taux.block<3,3>(0,0) = T.block<3,3>(0,0)*Ts.block<3,3>(0,0);
Taux.block<3,1>(0,3) = Ts.block<3,1>(0,3) + T.block<3,1>(0,3);
PrintCloudToPLY(ply_filename_s.data(), cloud_s, Ts);
PrintCloudToPLY(ply_filename_d.data(), cloud_d, Taux);
//std::cout << T<< "\n";
//std::cout << atan2(T(2,1), T(2,2)) << " " << asin(-T(2,0)) << " " << atan2(T(1,0), T(0,0)) << "\n";
}
Eigen::Matrix4d View::calcProjectionMatrix(Point2f frameBufferSize,float margin)
{
float near=0,far=0;
Point2d frustLL,frustUR;
frustLL.x() = -imagePlaneSize * (1.0 + margin);
frustUR.x() = imagePlaneSize * (1.0 + margin);
double ratio = ((double)frameBufferSize.y() / (double)frameBufferSize.x());
frustLL.y() = -imagePlaneSize * ratio * (1.0 + margin);
frustUR.y() = imagePlaneSize * ratio * (1.0 + margin);
near = nearPlane;
far = farPlane;
// Borrowed from the "OpenGL ES 2.0 Programming" book
Eigen::Matrix4d projMat;
Point3d delta(frustUR.x()-frustLL.x(),frustUR.y()-frustLL.y(),far-near);
projMat.setIdentity();
projMat(0,0) = 2.0f * near / delta.x();
projMat(1,0) = projMat(2,0) = projMat(3,0) = 0.0f;
projMat(1,1) = 2.0f * near / delta.y();
projMat(0,1) = projMat(2,1) = projMat(3,1) = 0.0f;
projMat(0,2) = (frustUR.x()+frustLL.x()) / delta.x();
projMat(1,2) = (frustUR.y()+frustLL.y()) / delta.y();
projMat(2,2) = -(near + far ) / delta.z();
projMat(3,2) = -1.0f;
projMat(2,3) = -2.0f * near * far / delta.z();
projMat(0,3) = projMat(1,3) = projMat(3,3) = 0.0f;
return projMat;
}
Eigen::Matrix4d Dataset_analyzer::Computetransformation(){
Eigen::Quaterniond qs_n = qs*qp; //change axis
Eigen::Quaterniond qd_n = qd*qp; //change axis
/*
Eigen::Matrix4d Ts; Ts.setIdentity();
Eigen::Matrix4d Td; Td.setIdentity();
Ts.block<3,3>(0,0) = qs_n.toRotationMatrix ();
Ts.block<3,1>(0,3) = ts;
Td.block<3,3>(0,0) = qd_n.toRotationMatrix ();
Td.block<3,1>(0,3) = td;
Eigen::Matrix4d T = Ts*Td.inverse();
*/
Eigen::Quaterniond q12 = qd_n*qs_n.inverse(); //compute rotation s --> d
Eigen::Matrix4d T; T.setIdentity();
T.block<3,3>(0,0) = q12.toRotationMatrix ();
T.block<3,1>(0,3) = td-ts;
return T;
}
demo(std::string _pointcloudtopic,std::string _cameras_filename){
T.setIdentity();
pub_pointcloud = n.advertise< sensor_msgs::PointCloud2 > ("/icp/world", 1);
sub_pointcloud = n.subscribe(_pointcloudtopic.data(), 1, &demo::get_cloud_Callback, this);
cameras_filename=_cameras_filename;
f_th=4;
cloud_key = PCloud(new Cloud());
}
TEST(TransformationTestSuite, testConstructor)
{
using namespace sm::kinematics;
Transformation T_a_b;
Eigen::Matrix4d T;
T.setIdentity();
sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");
}
Eigen::Matrix4d VertexPointXYZCov::covTransform(){
EigenSolver<Matrix3d> solv(_cov);
Matrix3d eigenVectors = solv.eigenvectors().real();
Eigen::Matrix4d covGlTransform;
covGlTransform.setIdentity();
for(int i=0; i < 3; ++i)
for(int j=0; j < 3; ++j)
covGlTransform(i,j) = eigenVectors(i,j);
for(int i=0; i < 3; ++i)
covGlTransform(i,3) = estimate()(i);
return covGlTransform;
}
TEST(UncertainTransformationTestSuite, testConstructor)
{
using namespace sm::kinematics;
UncertainTransformation T_a_b;
Eigen::Matrix4d T;
T.setIdentity();
UncertainTransformation::covariance_t U;
U.setZero();
sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");
sm::eigen::assertNear(U, T_a_b.U(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating zero uncertainty");
}