本文整理汇总了C++中eigen::Matrix4d::inverse方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix4d::inverse方法的具体用法？C++ Matrix4d::inverse怎么用？C++ Matrix4d::inverse使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Matrix4d的用法示例。
在下文中一共展示了Matrix4d::inverse方法的11个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: GetCameraCenterWorld
Eigen::Vector3d GetCameraCenterWorld() {
GLdouble projmat[16];
GLdouble modelmat[16];
glGetDoublev( GL_PROJECTION_MATRIX, projmat);
glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
Eigen::Matrix4d InvModelMat;
Eigen::Matrix4d InvProjMat;
InvModelMat.setZero();
InvProjMat.setZero();
for(int i=0; i<4; i++){
for(int j=0; j<4; j++){
InvModelMat(i,j)=modelmat[4*i+j];
InvProjMat(i,j)=projmat[4*i+j];
//printf("%lf ", InvModelMat[i][j]);
}
//printf("\n");
}
InvModelMat=(InvModelMat*InvProjMat).transpose();
InvModelMat = (InvModelMat.inverse());
Eigen::Vector4d cc(0, 0, 0, 1);
cc=InvModelMat*cc;
if(cc.norm()!=0) cc=cc/cc[3];
return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}示例2: _IterateCurvatureReduction
void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
double epsilon = 0.0001;
//create a jacobian for the parameters by perturbing them
Eigen::Vector4d Jt; //transpose of the jacobian
BezierBoundaryProblem origProblem = *pProblem;
double maxK = _GetMaximumCurvature(pProblem);
for(int ii = 0; ii < 4 ; ii++){
Eigen::Vector4d epsilonParams = params;
epsilonParams[ii] += epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kPlus = _GetMaximumCurvature(pProblem);
epsilonParams[ii] -= 2*epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kMinus = _GetMaximumCurvature(pProblem);
Jt[ii] = (kPlus-kMinus)/(2*epsilon);
}
//now that we have Jt, we can calculate JtJ
Eigen::Matrix4d JtJ = Jt*Jt.transpose();
//thikonov regularization
JtJ += Eigen::Matrix4d::Identity();
Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
params -= deltaParams;
_Get5thOrderBezier(pProblem,params);
_Sample5thOrderBezier(pProblem);
//double finalMaxK = _GetMaximumCurvature(pProblem);
//dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}示例3: Dist_grad
void Dist_grad(const Eigen::Matrix<double,Eigen::Dynamic,1> &t,
const std::vector<skeleton::BranchContProjSkel::Ptr> &skel,
double &value,
Eigen::Matrix<double,Eigen::Dynamic,1> &gradient)
{
Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
Eigen::Vector4d b = Eigen::Vector4d::Zero();
value = 0.0;
gradient = Eigen::Matrix<double,Eigen::Dynamic,1>::Zero(t.rows(),1);
std::vector<Eigen::Vector4d> ori(skel.size());
std::vector<Eigen::Vector4d> vec(skel.size());
std::vector<Eigen::Vector4d> orijac(skel.size());
std::vector<Eigen::Vector4d> vecjac(skel.size());
std::vector<Eigen::Matrix4d> A_part_jac(skel.size());
std::vector<Eigen::Vector4d> b_part_jac(skel.size());
for(unsigned int i=0;i<skel.size();i++)
{
Compositor<Application<Eigen::Matrix<double,8,1>,Eigen::Vector3d>,Application<Eigen::Vector3d,double> >
fun(skel[i]->getModel()->getR8Fun(),skel[i]->getCompFun());
Eigen::Matrix<double,8,1> veclin = fun(t(i));
Eigen::Matrix<double,8,1> jaclin = fun.jac(t(i));
ori[i] = veclin.block<4,1>(0,0);
vec[i] = veclin.block<4,1>(4,0).normalized();
orijac[i] = jaclin.block<4,1>(0,0);
vecjac[i] = jaclin.block<4,1>(4,0)*(1.0/veclin.block<4,1>(4,0).norm());
Eigen::Matrix4d A_part = Eigen::Matrix4d::Identity() - vec[i]*vec[i].transpose();
Eigen::Vector4d b_part = A_part*ori[i];
A_part_jac[i] = - vecjac[i]*vec[i].transpose() - vec[i]*vecjac[i].transpose();
b_part_jac[i] = A_part_jac[i] * ori[i] + A_part * orijac[i];
A+=A_part;
b+=b_part;
}
Eigen::Matrix4d A_inv = A.inverse();
Eigen::Vector4d P = A_inv*b;
for(unsigned int i=0;i<skel.size();i++)
{
Eigen::Vector4d P_jac = A_inv * A_part_jac[i] * P + A_inv * b_part_jac[i];
double Dist = (P - ori[i]).squaredNorm() - pow((P - ori[i]).dot(vec[i]),2);
gradient(i) = 2*(P_jac - orijac[i]).dot(P - ori[i])
-2*(P_jac - orijac[i]).dot(vec[i]) * (P - ori[i]).dot(vec[i])
-2*(P - ori[i]).dot(vecjac[i]) * (P - ori[i]).dot(vec[i]);
value += Dist;
}
}示例4: referencejpInput
SMC_higher_order(/*systems::ExecutionManager* em*/bool status,
const Eigen::Matrix4d coeff, const Eigen::Vector4d delta,
const Eigen::Matrix4d A, const Eigen::Matrix4d B, const float P,
const float Q,
const std::string& sysName = "Slidingmode_Controller") :
System(sysName), referencejpInput(this), referencejvInput(this), referencejaInput(
this), feedbackjpInput(this), feedbackjvInput(this), M(
this), C(this),
controlOutput(this, &controlOutputValue), STATUS(status), Coeff(
coeff), Delta(delta), A(A), B(B), P(P), Q(Q) {
inv_B = B.inverse();
}示例5: distance
double Keyframe::distance( const Eigen::Matrix4d & transform ) const
{
const Eigen::Matrix4d& poseMat = _pose.transformation();
const Eigen::Matrix4d relativePose = poseMat * transform.inverse();
double dist = relativePose.block<3, 1>( 0, 3 ).norm();
// const Eigen::Matrix3d & poseR = poseMat.block<3, 3>( 0, 0 );
// const Eigen::Matrix3d & kfR = transform.block<3, 3>( 0, 0 );
// Eigen::Matrix3d deltaR = poseR.transpose() * kfR;
// Eigen::Vector3d euler = deltaR.eulerAngles( 0, 1, 2 );
// dist += euler.norm();
return dist;
}示例6: GetPointEyeToWorld
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) {
GLdouble modelmat[16];
glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
Eigen::Matrix4d InvModelMat;
InvModelMat.setZero();
for(int i=0; i<4; i++){
for(int j=0; j<4; j++){
InvModelMat(i,j)=modelmat[4*i+j];
}
}
InvModelMat=(InvModelMat.transpose());
InvModelMat=(InvModelMat.inverse());
Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1);
cc=InvModelMat*cc;
if(cc.norm()!=0) cc=cc/cc[3];
return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}示例7: operate
virtual void operate() {
tmp_theta_pos = this->feedbackjpInput.getValue();
ThetaInput << tmp_theta_pos[0], tmp_theta_pos[1], tmp_theta_pos[2], tmp_theta_pos[3];
M_tmp = this->M.getValue();
Md_tmp = this->Md.getValue();
tmp_theta_vel = this->feedbackjvInput.getValue();
ThetadotInput << tmp_theta_vel[0], tmp_theta_vel[1], tmp_theta_vel[2], tmp_theta_vel[3];
C_tmp = this->C.getValue();
Cd_tmp = this->Cd.getValue();
// Reference position, velocity and accelerations
qd[0] = this->referencejpInput.getValue()[0];
qd[1] = this->referencejpInput.getValue()[1];
qd[2] = this->referencejpInput.getValue()[2];
qd[3] = this->referencejpInput.getValue()[3];
qdd[0] = this->referencejvInput.getValue()[0];
qdd[1] = this->referencejvInput.getValue()[1];
qdd[2] = this->referencejvInput.getValue()[2];
qdd[3] = this->referencejvInput.getValue()[3];
qddd[0] = this->referencejaInput.getValue()[0];
qddd[1] = this->referencejaInput.getValue()[1];
qddd[2] = this->referencejaInput.getValue()[2];
qddd[3] = this->referencejaInput.getValue()[3];
//
//Position error
e = ThetaInput - qd;
//Velocity error
ed = ThetadotInput - qdd;
//Eta
eta = K / b;
//The error matrix
Xtilde.resize(8, 1);
Xtilde << e, ed;
Md_tmpinverse = Md_tmp.inverse();
M_tmpinverse = M_tmp.inverse();
F = -M_tmpinverse * (C_tmp);
Fd = -Md_tmpinverse * (Cd_tmp);
rho = F - Fd;
// The fbar vector
for (i = 0; i < 4; i = i + 1) {
fbar[i] = rho[i] + eta[i] * c[i] * Xtilde[i]
+ (c[i] + eta[i]) * Xtilde[4 + i] + eta[i] * phi[i];
}
//
Ftilde1 << Xtilde[4], -eta[0] * c[0] * Xtilde[0]
- (c[0] + eta[0]) * Xtilde[4] - eta[0] * phi[0], 0;
Ftilde2 << Xtilde[5], -eta[1] * c[1] * Xtilde[1]
- (c[1] + eta[1]) * Xtilde[5] - eta[1] * phi[1], 0;
Ftilde3 << Xtilde[6], -eta[2] * c[2] * Xtilde[2]
- (c[2] + eta[2]) * Xtilde[6] - eta[2] * phi[2], 0;
Ftilde4 << Xtilde[7], -eta[3] * c[3] * Xtilde[3]
- (c[3] + eta[3]) * Xtilde[7] - eta[3] * phi[3], 0;
//The L and Lbar vectors
for (i = 0; i < 4; i = i + 1) {
L[i] = 0.5
* (Xtilde[i] * Xtilde[i] + Xtilde[4 + i] * Xtilde[4 + i]);
Lbar[i] = L[i] + fbar[i] * fbar[i];
}
// The gradient matrices
DVDX1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* W1[0], (W1[0] * Xtilde[0] + W1[1] * Xtilde[4]
+ W1[2] * phi[0]) * W1[1], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * W1[2];
DVDX2
<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
* W2[0], (W2[0] * Xtilde[1] + W2[1] * Xtilde[5]
+ W2[2] * phi[1]) * W2[1], (W2[0] * Xtilde[1]
+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * W2[2];
DVDX3
<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
* W3[0], (W3[0] * Xtilde[2] + W3[1] * Xtilde[6]
+ W3[2] * phi[2]) * W3[1], (W3[0] * Xtilde[2]
+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * W3[2];
DVDX4
<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
* W4[0], (W4[0] * Xtilde[3] + W4[1] * Xtilde[7]
+ W4[2] * phi[3]) * W4[1], (W4[0] * Xtilde[3]
+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * W4[2];
DVDW1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* Xtilde[0] + W1[0], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * Xtilde[4] + W1[1], (W1[0]
* Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0]) * phi[0]
+ W1[2];
DVDW2
//.........这里部分代码省略.........
示例8: addNewView
bool SurfelsRGBDModel::addNewView(const PCloud &cloud_d, Eigen::Matrix4d T_world){
pcl::KdTreeFLANN< pcl::PointXYZRGB > kdtree;
kdtree.setInputCloud (cloud_d);
std::vector<int> indice(1);
std::vector<float> distance(1);
std::vector<int> covered_pixels(cloud_d->size(),0);
Eigen::Vector4d auxp;
Eigen::Vector4d surfel_normal;
Eigen::Vector4d cloudd_normal;
pcl::PointXYZRGB surfel_pt;
int ptoremove=0;
pcl::PointSurfel remove_aux;
//update surfels
for(unsigned int i=0; i<m_surfels.size()-ptoremove; i++){
auxp << m_surfels[i].x, m_surfels[i].y, m_surfels[i].z, 1.0;
//std::cout << auxp.transpose() << "\n";
auxp = T_world.inverse()*auxp;
surfel_pt.x = auxp(0);
surfel_pt.y = auxp(1);
surfel_pt.z = auxp(2);
//std::cout << surfel_pt.x << " " << surfel_pt.y << " " << surfel_pt.z << "\n\n";
//checks if there is any point in range
if (kdtree.nearestKSearch (surfel_pt, 1, indice, distance) != 1) continue;
//std::cout << "depois tree " << distance[0] << " " << mindist*mindist << " " << mindist <<"\n";
if (distance[0]>mindist*mindist){
if (m_surfels[i].confidence < 2)
{
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}
continue;
}
//checks normal angles
surfel_normal << m_surfels[i].normal_x,m_surfels[i].normal_y, m_surfels[i].normal_z, 1.0;
cloudd_normal = computeNormal(indice[0],&kdtree,cloud_d);
surfel_normal =T_world.inverse()*surfel_normal;
normalize(surfel_normal);
float normal_angle = acos(cloudd_normal(0)*surfel_normal(0) + cloudd_normal(1)*surfel_normal(1) + cloudd_normal(2)*surfel_normal(2));
//std::cout << normal_angle*180.0/3.14159265 << " " << update_max_normal_angle << "\n";
if (normal_angle > (update_max_normal_angle*3.14159265/180.0))
{
// Removal check. If a surfel has a different normal and is closer to the camera
// than the new scan, remove it.
if ((surfel_pt.z > cloud_d->points[indice[0]].z) && (m_surfels[i].confidence < 2))
{
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}else
covered_pixels[indice[0]] = 1;
continue;
}
//checks distance
// If existing surfel is far from new depth value:
// - If existing one had a worst point of view, and was seen only once, remove it.
// - Otherwise do not include the new one.
if (std::fabs(surfel_pt.z - cloud_d->points[indice[0]].z) > update_max_dist){
if (m_surfels[i].confidence < 2){
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}
else
covered_pixels[indice[0]] = 1;
continue;
}
//.........这里部分代码省略.........
示例9: optimize
void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)
{
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//add param
mPoseMap.lock();
int length = localPoseMap.size();
// w^t_i w^q_i
double t_array[length][3];
double q_array[length][4];
map<double, vector<double>>::iterator iter;
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
t_array[i][0] = iter->second[0];
t_array[i][1] = iter->second[1];
t_array[i][2] = iter->second[2];
q_array[i][0] = iter->second[3];
q_array[i][1] = iter->second[4];
q_array[i][2] = iter->second[5];
q_array[i][3] = iter->second[6];
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],
iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],
iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);
problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
}
//gps factor
double t = iterVIO->first;
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end())
{
ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
iterGPS->second[2], iterGPS->second[3]);
//printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
}
}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
// update global pose
//mPoseMap.lock();
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
double t = iter->first;
WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],
//.........这里部分代码省略.........
示例10: colorize
static bool colorize(const drc::map_image_t& iDepthMap,
const Eigen::Affine3d& iLocalToCamera,
const bot_core::image_t& iImage,
const BotCamTrans* iCamTrans,
bot_core::image_t& oImage) {
oImage.utime = iDepthMap.utime;
oImage.width = iDepthMap.width;
oImage.height = iDepthMap.height;
oImage.row_stride = 3*iDepthMap.width;
oImage.pixelformat = PIXEL_FORMAT_RGB;
oImage.size = oImage.row_stride * oImage.height;
oImage.nmetadata = 0;
oImage.data.resize(oImage.size);
Eigen::Matrix4d xform;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
xform(i,j) = iDepthMap.transform[i][j];
}
}
xform = iLocalToCamera.matrix()*xform.inverse();
for (int i = 0; i < iDepthMap.height; ++i) {
for (int j = 0; j < iDepthMap.width; ++j) {
double z;
if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
if (z < -1e10) {
continue;
}
}
else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
if (val == 0) {
continue;
}
z = val;
}
else {
continue;
}
Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
double p[3] = {pt(0)/pt(3),pt(1)/pt(3),pt(2)/pt(3)};
double pix[3];
bot_camtrans_project_point(iCamTrans, p, pix);
if (pix[2] < 0) {
continue;
}
uint8_t r,g,b;
if (!interpolate(pix[0], pix[1], iImage, r, g, b)) {
continue;
}
int outImageIndex = i*oImage.row_stride + 3*j;
oImage.data[outImageIndex+0] = r;
oImage.data[outImageIndex+1] = g;
oImage.data[outImageIndex+2] = b;
}
}
return true;
}示例11: dataCallback
/*
* here comes the object cluster point cloud in camera frame of reference
*/
void dataCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud) {
if( !create_map_ )
return;
ROS_INFO("creating map");
object_mutex.lock();
ROS_INFO("dataCallback got the mutex");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudIn = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
pcl::fromROSMsg(*point_cloud, *pointCloudIn);
// Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();
// change reference frame of point cloud to point mean and oriented along principal axes
Eigen::Vector4d mean;
Eigen::Vector3d eigenvalues;
Eigen::Matrix3d cov;
Eigen::Matrix3d eigenvectors;
pcl::computeMeanAndCovarianceMatrix( *pointCloudIn, cov, mean );
pcl::eigen33( cov, eigenvectors, eigenvalues );
/*
* Added comment to this misterious sign change:
* Assuming the eigenvectors of the object come in the camera frame of reference
* this means the unit vector along the Z axis (Eigen::Vector3d::UnitZ()) points away from the camera.
* The LAST eigenvector points upwards/downwards. If their dot product is positive
* means the eigenvector points downwards. This sign change would
* normalize it so that it always points downwards
*
*/
// z eigenvector
if( Eigen::Vector3d(eigenvectors.col(0)).dot( Eigen::Vector3d::UnitZ() ) > 0.0 )
eigenvectors.col(0) = (-eigenvectors.col(0)).eval();
Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();
objectTransform.block<3,1>(0,0) = eigenvectors.col(2);
objectTransform.block<3,1>(0,1) = eigenvectors.col(1);
objectTransform.block<3,1>(0,2) = eigenvectors.col(0);
objectTransform.block<3,1>(0,3) = mean.block<3,1>(0,0);
if( objectTransform.block<3,3>(0,0).determinant() < 0 ) {
// x eigenvector (z axis)
objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
}
/*
* -----------------------------
* set roll and pitch to 0,
* leave only the yaw as a free parameter
*/
//flatten_roll_pitch(objectTransform);
/*
* ---------------------------
*/
eigenvectors.col(2) = objectTransform.block<3,1>(0,0); // x axis
eigenvectors.col(1) = objectTransform.block<3,1>(0,1); // y axis
eigenvectors.col(0) = objectTransform.block<3,1>(0,2); // z axis
if(normalise_transform(eigenvectors,point_cloud)){
// x eigenvector
objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
// y eigenvector
objectTransform.block<3,1>(0,1) = -objectTransform.block<3,1>(0,1);
}
// transform from camera frame of reference to object's main axes frame of reference
Eigen::Matrix4d objectTransformInv = objectTransform.inverse();
//the MRSmap of the object is stored in the frame of reference of the objects' eigen axis
pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectPointCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
pcl::transformPointCloud( *pointCloudIn, *objectPointCloud, (objectTransformInv).cast<float>() );
objectPointCloud->sensor_origin_ = objectTransformInv.block<4,1>(0,3).cast<float>();
objectPointCloud->sensor_orientation_ = Eigen::Quaternionf( objectTransformInv.block<3,3>(0,0).cast<float>() );
treeNodeAllocator_->reset();
map_ = boost::shared_ptr< MultiResolutionSurfelMap >( new MultiResolutionSurfelMap( max_resolution_, max_radius_, treeNodeAllocator_ ) );
map_->params_.dist_dependency = dist_dep_;
std::vector< int > pointIndices( objectPointCloud->points.size() );
for( unsigned int i = 0; i < pointIndices.size(); i++ ) pointIndices[i] = i;
map_->imageAllocator_ = imageAllocator_;
map_->addPoints( *objectPointCloud, pointIndices );
map_->octree_->root_->establishNeighbors();
//.........这里部分代码省略.........