void Point::optimize(const size_t n_iter)
{
Vector3d old_point = pos_;
double chi2 = 0.0;
Matrix3d A;
Vector3d b;
for(size_t i=0; i<n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// compute residuals
for(auto it=obs_.begin(); it!=obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J);
const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// solve linear system
const Vector3d dp(A.ldlt().solve(b));
// check if error increased
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
{
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
#endif
pos_ = old_point; // roll-back
break;
}
// update the model
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(b) = " << vk::norm_max(b)
<< endl;
#endif
// stop when converged
if(vk::norm_max(dp) <= EPS)
break;
}
#ifdef POINT_OPTIMIZER_DEBUG
cout << endl;
#endif
}
void skew_symmetric_for_cross(const Vector3d& vec, Matrix3d& skew_mat)
{
skew_mat.setZero();
skew_mat(0,1) = -vec(2);
skew_mat(1,0) = vec(2);
skew_mat(0,2) = vec(1);
skew_mat(2,0) = -vec(1);
skew_mat(1,2) = -vec(0);
skew_mat(2,1) = vec(0);
}
void Point3D::optimize(const size_t n_iter)
{
Vector3d old_point = pos_;
double chi2 = 0.0;
Matrix3d A;
Vector3d b;
for (size_t i = 0; i < n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// 计算残差
for (auto it = obs_.begin(); it != obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point3D::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
const Vector2d e(project2d((*it)->f) - project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// 求解线性系统
const Vector3d dp(A.ldlt().solve(b));
// 检测误差有没有增长
if ((i > 0 && new_chi2 > chi2) || (bool)std::isnan((double)dp[0]))
{
pos_ = old_point; // 回滚
break;
}
// 更新模型
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
// 收敛则停止
if (norm_max(dp) <= EPS)
break;
}
}
void
SoftBody::computeFs(uint32_t lo, uint32_t hi)
{
auto n_it = neighborhoods.begin() + lo;
auto f_it = defs.begin() + lo;
auto u_it = posWorld.begin() + lo;
auto b_it = bases.begin() + lo;
auto end = posWorld.begin() + hi;
Matrix3d rhs;
for (; u_it != end; ++u_it, ++f_it, ++n_it, ++b_it) {
rhs.setZero();
for (auto& n : *n_it) {
rhs += n.w * (posWorld[n.index] - *u_it) * n.u.transpose();
}
*f_it = Svd(rhs * *b_it, ComputeFullU | ComputeFullV);
}
}
void RGBDFramePair::calTransFromFPoints(const vector<int>& index,MatrixXd& trans)
{
Vector3d rCenter(0,0,0);
Vector3d mCenter(0,0,0);
Matrix3d mat;
mat.setZero();
int cSize=index.size();
for (int j=0;j<cSize;j++)
{
rCenter+=refFPoints[index[j]];
mCenter+=mainFPoints[index[j]];
}
rCenter/=cSize;
mCenter/=cSize;
for (int j=0;j<cSize;j++)
{
Vector3d r=refFPoints[index[j]];
Vector3d m=mainFPoints[index[j]];
mat+=(m-mCenter)*(r-rCenter).transpose();
}
HMatrix hmat;
HMatrix Q,S;
copyHMatrix3D(hmat,mat);
polar_decomp(hmat,Q,S);
trans.resize(3,4);
trans.setZero();
MatrixXd trans3D(3,3);
Vector3d translation;
copyFromHMatrix3D(Q,trans3D);
copyFromHMatrix3D(Q,trans);
translation=mCenter-trans3D*rCenter;
trans.col(3)=translation;
}
void Voxelize::generateUmap(pcl::PointCloud<pcl::PointXYZ> &cloud,double resolution,boost::unordered_map<unordered_map_voxel,un_key> &m)//遍历每个点，生成栅格。然后计算每个栅格的参数
{
double t0 = ros::Time::now().toSec();
un_key tempp; //key键不允许修改，全存在value中
for(int i=0;i<cloud.size();i++)
{
unordered_map_voxel tempv(cloud[i].x,cloud[i].y,cloud[i].z,resolution);
pair<umap::iterator,bool> pair_insert = m.insert(umap::value_type(tempv,tempp));//这个pair就是用来存储插入返回值的，看是否这个栅格已经存在
pair_insert.first->second.vec_index_point.push_back(i);
/*
if(!pair_insert.second)//如果栅格已经存在，那么把value++
{
(pair_insert.first)->second.cout++;
pair_insert.first->second.vec_index_point.push_back(i);
}
else{
pair_insert.first->second.vec_index_point.push_back(i);
}
*/
}
double t2 = ros::Time::now().toSec();
//cout <<"划栅格的时间为"<<(t2-t0)<<endl;
Matrix3d tempmat;
tempmat.setZero();
pcl::PointXYZ temppoint(0,0,0);
for(umap::iterator iter=m.begin();iter!=m.end();)
{
temppoint.x=temppoint.y=temppoint.z=0;
if(iter->second.vec_index_point.size()<10)
{
m.erase(iter++);
continue;
}
for(int j = 0;j<iter->second.vec_index_point.size();j++)
{
double x_ = cloud[iter->second.vec_index_point[j]].x;
double y_ = cloud[iter->second.vec_index_point[j]].y;
double z_ = cloud[iter->second.vec_index_point[j]].z;
tempmat += (Vector3d(x_,y_,z_) * RowVector3d(x_,y_,z_));//存储pipiT的和
temppoint.x += x_;
temppoint.y += y_;
temppoint.z += z_;
}
int n = iter->second.vec_index_point.size();
//cout <<"栅格大小为"<<n<<endl;
Vector3d v(temppoint.x/n,temppoint.y/n,temppoint.z/n);//存储栅格重心
RowVector3d vT(temppoint.x/n,temppoint.y/n,temppoint.z/n);
tempmat /= n;
tempmat -= v*vT;//S
iter->second.matS = tempmat;
//cout <<"S 矩阵= "<< tempmat <<endl;
vector<eigen_sort> vector1(3);//用于排序
//cout <<"tempmat="<<endl<<tempmat<<endl;
EigenSolver<MatrixXd> es(tempmat);
VectorXcd eivals = es.eigenvalues();
MatrixXcd eigenvectors = es.eigenvectors();
//cout << "特征值="<<eivals<<endl;
vector1[0]=eigen_sort(eivals(0).real(),es.eigenvectors().col(0));
vector1[1]=eigen_sort(eivals(1).real(),es.eigenvectors().col(1));
vector1[2]=eigen_sort(eivals(2).real(),es.eigenvectors().col(2));
sort(vector1.begin(),vector1.end());
double lamada1 = vector1[0].eigen_value;
double lamada2 = vector1[1].eigen_value;
double lamada3 = vector1[2].eigen_value;
//cout <<"lamada="<<lamada1<<" "<<lamada2<<" "<<lamada3<<endl;
//
if(vector1[0].eigen_vector(2).real()<0)
{
vector1[0].eigen_vector(2).real() = -vector1[0].eigen_vector(2).real();
}
if(vector1[2].eigen_vector(2).real()<0)
{
vector1[2].eigen_vector(2).real() = -vector1[2].eigen_vector(2).real();
}
iter->second.c = (lamada3-lamada2)/(lamada1+lamada2+lamada3);
iter->second.p = 2*(lamada2-lamada1)/(lamada1+lamada2+lamada3);
iter->second.u = v;
iter->second.v1 = vector1[0].eigen_vector;
iter->second.v3 = vector1[2].eigen_vector;
double c = iter->second.c;
double p = iter->second.p;
iter->second.vector_9D << v(0),v(1),v(2),p*vector1[0].eigen_vector(0).real(),p*vector1[0].eigen_vector(1).real(),
p*vector1[0].eigen_vector(2).real(),c*vector1[2].eigen_vector(0).real(),c*vector1[2].eigen_vector(1).real(),
p*vector1[2].eigen_vector(2).real();
//cout << "9D向量="<<iter->second.vector_9D<<endl;
//ArrayXd ad = iter->second.vector_9D;
//cout <<"ad = "<<ad<<endl;
//cout <<"ad square="<<ad.square()<<endl;
tempmat.setZero();
iter++;
}
double tn = ros::Time::now().toSec();
//cout << "对栅格计算处理的时间"<<(tn - t2)<<endl;
//cout <<"栅格数量为" <<m.size()<<endl;
}
void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");
const double dLength = length();
//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
double dVar = variance();
dVar = std::max<double>(1e-5, dVar);
normalisedPose.scale(dLength).write(toroGraphFile, b2d);
Vector3d t, x_axis; x_axis.setZero();
normalisedPose.t.asVector(t);
if(b2d)
{
//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
Vector2d t2d(t(0), t(2));
Vector2d t2d_perp(t(2), -t(0));
Matrix2d Q, LAMBDA;
LAMBDA.setZero();
Q << t2d, t2d_perp;
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix2d C = Q * LAMBDA * Q.transpose();
const Matrix2d & I = C.inverse();
//cout << I << endl;
//THROW("Not complete...")
double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";
return;
}
if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
x_axis(0) = 1;
else
x_axis(1) = 1;
Matrix3d Q;
if(t.sum() == 0) //Pure rotation. Should be arbitrary
{
Q.setIdentity();
}
else
{
Vector3d tperp1 = t.cross(x_axis);
tperp1.normalize();
Vector3d tperp2 = tperp1.cross(t);
Q << t, tperp1, tperp2;
}
CHECK(isnan(Q.sum()), "writeEdge: nan")
Matrix3d LAMBDA; LAMBDA.setZero();
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix3d C = Q * LAMBDA * Q.transpose();
//std::cout << "Covariance: " << std::endl << C << std::endl;
CHECK(isnan(C.sum()), "writeEdge: nan")
Matrix3d Crpy; Crpy.setZero();
Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;
CHECK(isnan(Cfull.sum()), "writeEdge: nan")
if(Cfull.trace() < 0.0001)
{
std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
Cfull.diagonal().array() += 0.0001;
}
const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
//std::cout << "Information: " << std::endl << Ifull << std::endl;
//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");
CHECK(isnan(Ifull.sum()), "writeEdge: nan")
for (int nRow = 0; nRow <6; nRow++)
{
for(int nCol = nRow; nCol < 6; nCol++)
toroGraphFile << Ifull(nRow, nCol) << ' ';
}
}
