C++ Matrix3d::setZero方法代码示例

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(),
//.........这里部分代码省略.........

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) << ' ';
	}
//.........这里部分代码省略.........