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