//*************************************************************************************************************************
std::string wholeBodyReach::jointToString(const Eigen::VectorXd &j, int precision)
{
    if(j.size()!=ICUB_DOFS && j.size()!=ICUB_DOFS+6)
        cout<<"Error in size of joint vector: "<<j.size()<<endl;
    
    int index=0;
    string ret = "";
    char tmp[350];
    if(j.size()==ICUB_DOFS+6)
    {
        ret += "base(";
        for(int i=0;i<6;i++)
        {
            sprintf(tmp, "% .*lf ", precision, j(index));
            ret+=tmp;
            index++;
        }
        ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
        ret += ")\t";
    }
    ret += "torso(";
    for(int i=0;i<3;i++)
    {
        sprintf(tmp, "% .*lf ", precision, j(index));
        ret+=tmp;
        index++;
    }
    ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
    ret += ")\tl_arm(";
    for(int i=0;i<5;i++)
    {
        sprintf(tmp, "% .*lf ", precision, j(index));
        ret+=tmp;
        index++;
    }
    ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
    ret += ")\tr_arm(";
    for(int i=0;i<5;i++)
    {
        sprintf(tmp, "% .*lf ", precision, j(index));
        ret+=tmp;
        index++;
    }
    ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
    ret += ")\tl_leg(";
    for(int i=0;i<6;i++)
    {
        sprintf(tmp, "% .*lf ", precision, j(index));
        ret+=tmp;
        index++;
    }
    ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
    ret += ")\tr_leg(";
    for(int i=0;i<6;i++)
    {
        sprintf(tmp, "% .*lf ", precision, j(index));
        ret+=tmp;
        index++;
    }
    ret += ")";
    return ret;
}

//.........这里部分代码省略.........
          }
        }
        static double nearest_r = 0.05
            * distanceFunction(tgi.last_s, tgi.last_g);
        ///	Get a random state
        bool r_ok = false;
        do
        {
          sampler_->sampleUniform(rmotion->state);
          r_ok = si_->getStateValidityChecker()->isValid(rmotion->state);
        } while (!r_ok && ptc == false);
        Motion *nearest_s = tStart_->nearest(rmotion);
        Motion *nearest_g = tGoal_->nearest(rmotion);
        Motion *last_valid_motion = new Motion(si_);
        std::pair<ompl::base::State*, double> last_valid(
            last_valid_motion->state, 0);

        Motion *new_s = NULL;
        Motion *new_g = NULL;
        ///	Connect random node to start tree
        bool succ_s = si_->checkMotion(nearest_s->state, rmotion->state,
            last_valid);
        if (succ_s)
        {
          Motion *motion = new Motion(si_);
          si_->copyState(motion->state, rmotion->state);
          motion->parent = nearest_s;
          tStart_->add(motion);
          new_s = motion;
        }
        else
        {
          if (last_valid.second == 0) last_valid_motion->state = NULL;
          Eigen::VectorXd eigen_g((int) si_->getStateDimension());
          memcpy(eigen_g.data(),
              rmotion->state->as<ompl::base::RealVectorStateSpace::StateType>()->values,
              sizeof(double) * eigen_g.rows());
          local_map_->jointRef = eigen_g;
          local_solver_->getProblem()->setTau(1e-4);
          Motion *new_motion = new Motion(si_);
          if (localSolve(nearest_s, last_valid_motion->state, new_motion))
          {
            new_s = new_motion;
            tStart_->add(new_motion);
            succ_s = true;
          }
          else if (new_motion->internal_path)
          {
            si_->copyState(rmotion->state, new_motion->state);
            bool addNew = true;
//						std::vector<Motion*> n_motions;
//						tStart_->nearestR(new_motion, nearest_r, n_motions);
//						for (int i = 0; i < n_motions.size(); i++)
//							if (!n_motions[i]->global_valid_)
//							{
//								addNew = false;
//								break;
//							}
            if (addNew)
            {
              new_motion->global_valid_ = false;
              tStart_->add(new_motion);
              si_->copyState(rmotion->state, new_motion->state);
            }
          }
        }

RcppExport SEXP svds_gen(SEXP A_mat_r, SEXP m_scalar_r, SEXP n_scalar_r,
                         SEXP k_scalar_r, SEXP nu_scalar_r, SEXP nv_scalar_r,
                         SEXP params_list_r, SEXP mattype_scalar_r)
{
    BEGIN_RCPP

    Rcpp::List params_svds(params_list_r);

    int m        = as<int>(m_scalar_r);
    int n        = as<int>(n_scalar_r);
    int k        = as<int>(k_scalar_r);
    int nu       = as<int>(nu_scalar_r);
    int nv       = as<int>(nv_scalar_r);
    int ncv      = as<int>(params_svds["ncv"]);
    double tol   = as<double>(params_svds["tol"]);
    int maxitr   = as<int>(params_svds["maxitr"]);
    int mattype  = as<int>(mattype_scalar_r);

    // Operation for original matrix
    MatProd* op_orig = get_mat_prod_op(A_mat_r, m, n, params_list_r, mattype);
    // Operation for SVD
    MatProd* op;
    if(m > n)
        op = new SVDTallOp(op_orig);
    else
        op = new SVDWideOp(op_orig);

    SymEigsSolver<double, LARGEST_ALGE, MatProd> eigs(op, k, ncv);
    eigs.init();
    int nconv = eigs.compute(maxitr, tol);
    if(nconv < k)
        Rcpp::warning("only %d singular values converged, less than k = %d", nconv, k);

    nu = std::min(nu, nconv);
    nv = std::min(nv, nconv);

    Eigen::VectorXd evals = eigs.eigenvalues();
    Eigen::MatrixXd evecs = eigs.eigenvectors(std::max(nu, nv));

    Rcpp::NumericVector d(nconv);
    Rcpp::NumericMatrix u(m, nu), v(n, nv);
    int nops = 0;

    // Copy evals to d and take the square root
    std::copy(evals.data(), evals.data() + nconv, d.begin());
    std::transform(d.begin(), d.end(), d.begin(), simple_sqrt);

    // Copy evecs to u or v according to the shape of A
    // If A is tall, copy evecs to v, otherwise copy to u
    if(m > n)
        std::copy(evecs.data(), evecs.data() + nv * n, v.begin());
    else
        std::copy(evecs.data(), evecs.data() + nu * m, u.begin());

    // Calculate the other one
    if(m > n)
    {
        // A = UDV', A'A = VD^2V', AV = UD, ui = A * vi / di
        // evecs has already been copied to v, so we can overwrite evecs
        for(int i = 0; i < nu; i++)
        {
            evecs.col(i) /= d[i];
            op_orig->perform_op(&evecs(0, i), &u(0, i));
            nops++;
        }
    } else {
        // A = UDV', AA' = UD^2U', A'U = VD, vi = A' * ui / di
        // evecs has already been copied to u, so we can overwrite evecs
        for(int i = 0; i < nv; i++)
        {
            evecs.col(i) /= d[i];
            op_orig->perform_tprod(&evecs(0, i), &v(0, i));
            nops++;
        }
    }

    Rcpp::RObject u_ret;
    Rcpp::RObject v_ret;

    if(nu > 0)  u_ret = u;  else  u_ret = R_NilValue;
    if(nv > 0)  v_ret = v;  else  v_ret = R_NilValue;

    delete op;
    delete op_orig;

    return Rcpp::List::create(
        Rcpp::Named("d")     = d,
        Rcpp::Named("u")     = u_ret,
        Rcpp::Named("v")     = v_ret,
        Rcpp::Named("niter") = eigs.num_iterations(),
        Rcpp::Named("nops")  = eigs.num_operations() * 2 + nops
    );

    END_RCPP
}

TEST(PinholeCamera, functions)
{
  const size_t NUM_POINTS = 100;

  // instantiate all possible versions of test cameras
  std::vector<std::shared_ptr<okvis::cameras::CameraBase> > cameras;
  cameras.push_back(
      okvis::cameras::PinholeCamera<okvis::cameras::NoDistortion>::createTestObject());
  cameras.push_back(
      okvis::cameras::PinholeCamera<
          okvis::cameras::RadialTangentialDistortion>::createTestObject());
  cameras.push_back(
      okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>::createTestObject());
  cameras.push_back(
      okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>::createTestObject());

  for (size_t c = 0; c < cameras.size(); ++c) {
    //std::cout << "Testing " << cameras.at(c)->type() << std::endl;
    // try quite a lot of points:
    for (size_t i = 0; i < NUM_POINTS; ++i) {
      // create a random point in the field of view:
      Eigen::Vector2d imagePoint = cameras.at(c)->createRandomImagePoint();

      // backProject
      Eigen::Vector3d ray;
      EXPECT_TRUE(cameras.at(c)->backProject(imagePoint, &ray));

      // randomise distance
      ray.normalize();
      ray *= (0.2 + 8 * (Eigen::Vector2d::Random()[0] + 1.0));

      // project
      Eigen::Vector2d imagePoint2;
      Eigen::Matrix<double, 2, 3> J;
      Eigen::Matrix2Xd J_intrinsics;
      EXPECT_TRUE(
          cameras.at(c)->project(ray, &imagePoint2, &J, &J_intrinsics)
              == okvis::cameras::CameraBase::ProjectionStatus::Successful);

      // check they are the same
      EXPECT_TRUE((imagePoint2 - imagePoint).norm() < 0.01);

      // check point Jacobian vs. NumDiff
      const double dp = 1.0e-7;
      Eigen::Matrix<double, 2, 3> J_numDiff;
      for (size_t d = 0; d < 3; ++d) {
        Eigen::Vector3d point_p = ray
            + Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
                              d == 2 ? dp : 0);
        Eigen::Vector3d point_m = ray
            - Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
                              d == 2 ? dp : 0);
        Eigen::Vector2d imagePoint_p;
        Eigen::Vector2d imagePoint_m;
        cameras.at(c)->project(point_p, &imagePoint_p);
        cameras.at(c)->project(point_m, &imagePoint_m);
        J_numDiff.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
      }
      EXPECT_TRUE((J_numDiff - J).norm() < 0.0001);

      // check intrinsics Jacobian
      const int numIntrinsics = cameras.at(c)->noIntrinsicsParameters();
      Eigen::VectorXd intrinsics;
      cameras.at(c)->getIntrinsics(intrinsics);
      Eigen::Matrix2Xd J_numDiff_intrinsics;
      J_numDiff_intrinsics.resize(2,numIntrinsics);
      for (int d = 0; d < numIntrinsics; ++d) {
        Eigen::VectorXd di;
        di.resize(numIntrinsics);
        di.setZero();
        di[d] = dp;
        Eigen::Vector2d imagePoint_p;
        Eigen::Vector2d imagePoint_m;
        Eigen::VectorXd intrinsics_p = intrinsics+di;
        Eigen::VectorXd intrinsics_m = intrinsics-di;
        cameras.at(c)->projectWithExternalParameters(ray, intrinsics_p, &imagePoint_p);
        cameras.at(c)->projectWithExternalParameters(ray, intrinsics_m, &imagePoint_m);
        J_numDiff_intrinsics.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
      }
      /*std::cout<<J_numDiff_intrinsics<<std::endl;
      std::cout<<"----------------"<<std::endl;
      std::cout<<J_intrinsics<<std::endl;
      std::cout<<"================"<<std::endl;*/
      EXPECT_TRUE((J_numDiff_intrinsics - J_intrinsics).norm() < 0.0001);

    }
  }
}

void KRAveragerCis::add_force_constant_vector(const Eigen::VectorXd & ks, const Eigen::VectorXd & rs) {
    for (int i = 0; i < ks.rows(); ++i) {
        add_force_constant_tuple(ks(i), rs(i));
    }
}

 inline vcl_size_t size(Eigen::VectorXd const & v) { return v.rows(); }

int main(int argc, char** argv)
{
  ros::init(argc, argv, "parser_test");
  ros::NodeHandle nh;

  try
  {
    std::string name = "youbot";
    RobotPtr robot = RobotPtr(new Robot(name));

    ParserPtr parser = ParserPtr(new Parser());
    std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/youbot.yaml";
    parser->load(path, robot);

    robot->getMobility()->print();

    ros::MultiThreadedSpinner spinner;

    TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());

    const std::string mnp_name = "mnp";
    unsigned long cnt = 0;
    const double period = 0.001;
    ros::Rate r(1 / period);

    ahl_filter::DifferentiatorPtr differentiator = ahl_filter::DifferentiatorPtr(new ahl_filter::PseudoDifferentiator(period, 1.0));

    Eigen::VectorXd q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
    Eigen::VectorXd dq = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
    differentiator->init(q, dq);

    Eigen::VectorXd pre_q = q;

    //std::ofstream ofs1;
    //ofs1.open("result1.hpp");
    //std::ofstream ofs2;
    //ofs2.open("result2.hpp");
    //std::ofstream ofs3;
    //ofs3.open("result3.hpp");

    while(ros::ok())
    {
      q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 1.0);
      double coeff = 1.0 * sin(2.0 * M_PI * 0.1 * cnt * period);
      ++cnt;

      q = coeff * q;

      //q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), M_PI / 2.0);

      q.coeffRef(0) = 0.0;
      q.coeffRef(1) = 0.0;
      q.coeffRef(2) = 0.0;

      robot->update(mnp_name, q);
      robot->computeBasicJacobian(mnp_name);
      robot->computeMassMatrix(mnp_name);

      differentiator->apply(q);

      Eigen::VectorXd dq1 = robot->getJointVelocity(mnp_name);

      Eigen::VectorXd dq2;
      differentiator->copyDerivativeValueTo(dq2);

      std::cout << "p     : " << q.transpose() << std::endl;
      std::cout << "pre_p : " << pre_q.transpose() << std::endl;

      Eigen::VectorXd dq3 = (q - pre_q) / period;
      pre_q = q;

      //std::cout << "dq1 : " << dq1.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << "dq2 : " << dq2.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << "dq3 : " << dq3.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
      //std::cout << dq << std::endl << std::endl;
      //std::cout << cos(2.0 * M_PI * 0.1 * cnt * 0.1) << std::endl;

      tf_publisher->publish(robot, false);

/*
      ofs1 << cnt * period << " ";
      ofs2 << cnt * period << " ";
      ofs3 << cnt * period << " ";
      for(unsigned int i = 0; i < dq.rows() - 3; ++i)
      {
        ofs1 << dq1[i + 3] << " ";
        ofs2 << dq2[i + 3] << " ";
        ofs3 << dq3[i + 3] << " ";
      }
      ofs1 << std::endl;
      ofs2 << std::endl;
      ofs3 << std::endl;
*/
      r.sleep();
    }
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
//.........这里部分代码省略.........

//==============================================================================
void NullFunction::evalGradient(const Eigen::VectorXd& _x,
                                Eigen::Map<Eigen::VectorXd> _grad)
{
  _grad.resize(_x.size());
  _grad.setZero();
}

//==============================================================================
void Problem::setUpperBounds(const Eigen::VectorXd& _ub)
{
  assert(static_cast<std::size_t>(_ub.size()) == mDimension && "Invalid size.");
  mUpperBounds = _ub;
}

		double Statistics::scalarProduct(const Eigen::VectorXd& cv)
		{
			return cv.dot(cv);
		}

int main(int argc, char **argv) {
  Random::randomize();
  std::string paramfile = "multi.ini";
  int test = 1;

  char c;
  while ((c = getopt(argc, argv, "p:h")) != EOF) {
    switch (c) {
      case 'p':
        paramfile = optarg;
        break;
      case 'h':
      default:
        std::cerr << "Usage: " << argv[0] << " [options]\n";
        std::cerr << "\nOptions:\n";
        std::cerr << "-p <file>:  use the given parameter file\n";
        std::cerr << "-h:         this help\n";
        return 1;
    }
  }

  TParam p;
  p.loadTree(paramfile);

  MultiAgentMotionModel *motionModel;
  std::string typestr = p("multi_rotor_control/type").toString();
  if (typestr == "point2d") {
    motionModel = new Point2dMotionModel();
  } else if (typestr == "rotor2d") {
    motionModel = new Rotor2dMotionModel();
  } else {
    std::cerr << "Error: Unknown type '" << typestr << "' - exiting\n";
    exit(-1);
  }
  motionModel->init(p);

  TargetTrackingController ttc;
  ttc.init(p);
  std::vector<const SensorModel*> sensorModels = ttc.getTopology().getSensorModels();

  if (test == 1) {
    // online test
    TargetTrajectory tt;
    tt.init(p);
    EKF ekf(motionModel);
    ekf.init(p);

    std::ofstream topo_out("topo.out");
    std::ofstream multi_out("multi.out");
    unsigned int nA = motionModel->getNumAgents();
    unsigned int nT = motionModel->getNumTargets();
    unsigned int aSD = motionModel->getAgentStateDim();
    unsigned int cSD = motionModel->getAgentControlDim();
    unsigned int tSD = motionModel->getTargetStateDim();

    // test output
    Eigen::VectorXd state = p("estimation/initialState").toVectorXd();
    for (unsigned int i=0; !tt.atEnd(); ++i) {
      Eigen::VectorXd mean = ekf.getMean();
      Eigen::MatrixXd cov = ekf.getCovariance();
//      Eigen::VectorXd control(nA*cSD);
//      Eigen::VectorXd targetPosition = state.segment(aSD*nA, cSD);
//      for (unsigned int i=0; i<nA; ++i) {
//        control.segment(cSD*i, cSD) = (targetPosition - state.segment(aSD*i, cSD))/p("estimation/motionDt").toDouble();
//      }
      Eigen::VectorXd control = ttc.getControlTopo(&ekf, motionModel, sensorModels);
      multi_out << aSD << " " << cSD << " " << tSD << " 0 0 " << nA << " " << nT << " 0 0 0"
          << " " << state.transpose()
          << " " << control.transpose()
          << " " << mean.transpose();
      multi_out << " " << Eigen::Map<Eigen::MatrixXd>(cov.data(), 1, cov.cols()*cov.cols());
//      for (int i=0; i<N+1; ++i) {
//        multi_out << " " << cov(2*agentStateDim, 2*agentStateDim) << " " << cov(2*agentStateDim, 2*agentStateDim+1) << " " << cov(2*agentStateDim+1, 2*agentStateDim) << " " << cov(2*agentStateDim+1, 2*agentStateDim+1);
//      }
      multi_out << "\n";
      ttc.getTopology().write(topo_out, state);
      // simulation
      state = motionModel->move(state, control, motionModel->sampleNoise(state, control));
      if (Random::uniform() < p("multi_rotor_control/kidnap/target").toDouble()) {
        // kidnap target
        state.segment(aSD*nA, 2) = tt.randomJump();
        ekf.getCovariance().block(aSD*nA, aSD*nA, 4, 4) = Eigen::MatrixXd::Identity(4, 4)*4.0;
      } else {
        state.segment(aSD*nA, 2) = tt.step();
      }
      if (Random::uniform() < p("multi_rotor_control/kidnap/agent").toDouble()) {
        // kidnap agent
        int agent = rand()%nA;
        state.segment(aSD*agent, 2) = Eigen::Vector2d(Random::uniform(-1, 1), Random::uniform(-0.5, 0.5));
        ekf.getCovariance().block(aSD*agent, aSD*agent, 2, 2) = Eigen::MatrixXd::Identity(2, 2)*4.0;
      }
      // state estimation
      ekf.predict(control);
      for (std::vector<const SensorModel*>::const_iterator it = sensorModels.begin();
          it != sensorModels.end(); ++it) {
        if ((*it)->measurementAvailable(state)) {
          Eigen::VectorXd noiseSample = (*it)->sampleNoise(state, (*it)->sense(state));
          Eigen::VectorXd measurementSample = (*it)->sense(state, noiseSample);
          ekf.correct(measurementSample, *(*it));
        }
//.........这里部分代码省略.........

		void KPLSModel::train()
		{	
			if (descriptor_matrix_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!"); 
			}
		// 	if (descriptor_matrix_.cols() < no_components_)
		// 	{
		// 		throw Exception::TooManyPLSComponents(__FILE__, __LINE__, no_components_, descriptor_matrix_.cols());
		// 	}
			
			kernel->calculateKernelMatrix(descriptor_matrix_, K_);

			Eigen::MatrixXd P;  // Matrix P saves all vectors p

			int cols = K_.cols();
			
			// determine the number of components that are to be created.
			// no_components_ contains the number of components desired by the user, 
			// but obviously we cannot calculate more PLS components than features
			int features = descriptor_matrix_.cols();
			unsigned int components_to_create = no_components_;
			if (features < no_components_) components_to_create = features; 

			U_.resize(K_.rows(), components_to_create);
			loadings_.resize(cols, components_to_create);
			weights_.resize(Y_.cols(), components_to_create);
			latent_variables_.resize(K_.rows(), components_to_create);
			P.resize(cols, components_to_create);
			
			Eigen::VectorXd w;
			Eigen::VectorXd t;
			Eigen::VectorXd c;
			Eigen::VectorXd u = Y_.col(0);

			Eigen::VectorXd u_old;
			
			for (unsigned int j = 0; j < components_to_create; j++)
			{
				for (int i = 0; i < 10000 ; i++)
				{	
					w = K_.transpose()*u / Statistics::scalarProduct(u);
					w = w / Statistics::euclNorm(w);
					t = K_*w;
					c = Y_.transpose()*t / Statistics::scalarProduct(t);
					u_old = u;
					u = Y_*c / Statistics::scalarProduct(c); 
				
					if (Statistics::euclDistance(u, u_old)/Statistics::euclNorm(u) > 0.0000001) 
					{ 
						continue;				
					}
					else  // if u has converged
					{
						break;
					}
				}

				Eigen::VectorXd p = K_.transpose() * t / Statistics::scalarProduct(t);
				K_ -= t * p.transpose();

				U_.col(j) = u;
				loadings_.col(j) = w;
				weights_.col(j) = c;
				P.col(j) = p;
				latent_variables_.col(j) = t;
			}

		// 	try // p's are not orthogonal to each other, so that in rare cases P.t()*loadings_ is not invertible
		// 	{
		// 		loadings_ = loadings_*(P.t()*loadings_).i();
		// 	}
		// 	catch(BALL::Exception::MatrixIsSingular e)
		// 	{
		// 		Eigen::MatrixXd I; I.setToIdentity(P.cols());
		// 		I *= 0.001;
		// 		loadings_ = loadings_*(P.t()*loadings_+I).i();
		// 	}

			Eigen::MatrixXd m = P.transpose()*loadings_;
			training_result_ = loadings_*m.colPivHouseholderQr().solve(weights_.transpose());

			calculateOffsets();
		}

 std::string vectorString(const Eigen::VectorXd& m)
 {
   // Make a string from the bytes
   return std::string((char *) m.data(), m.size() * sizeof(m(0)));
 }

	void test_schur_dense_vs_sparse(
		const TGraphInitRandom  *init_random,
		const TGraphInitManual  *init_manual,
		const double lambda = 1e3 )
	{
		// A linear system object holds the sparse Jacobians for a set of observations.
		my_rba_t::rba_problem_state_t::TLinearSystem  lin_system;

		size_t nUnknowns_k2k=0, nUnknowns_k2f=0;

		if (init_random)
		{
			randomGenerator.randomize(init_random->random_seed);
			nUnknowns_k2k=init_random->nUnknowns_k2k;
			nUnknowns_k2f=init_random->nUnknowns_k2f;
		}
		else
		{
			nUnknowns_k2k=init_manual->nUnknowns_k2k;
			nUnknowns_k2f=init_manual->nUnknowns_k2f;
		}

		// Fill example Jacobians for the test:
		//  * 2 keyframes -> 1 k2k edge (edges=unknowns)
		//  * 6 features with unknown positions.
		//  * 6*2 observations: each feature seen once from each keyframe
		// Note: 6*2*2 = 24 is the minimum >= 1*6+3*6=24 unknowns so
		//   Hessians are invertible
		// -----------------------------------------------------------------
		// Create observations:
		// Don't populate the symbolic structure, just the numeric part.
        static char valid_true = 1; // Just to initialize valid bit pointers to this one.
		{

			lin_system.dh_dAp.setColCount(nUnknowns_k2k);
			lin_system.dh_df.setColCount(nUnknowns_k2f);
			size_t idx_obs = 0;
			for (size_t nKF=0;nKF<=nUnknowns_k2k;nKF++)
			{
				my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_dAp::col_t * dh_dAp_i = (nKF==0) ? NULL : (&lin_system.dh_dAp.getCol(nKF-1));

				for (size_t nLM=0;nLM<nUnknowns_k2f;nLM++)
				{
					// Do we have an observation of feature "nLM" from keyframe "nKF"??
					bool add_this;

					if (init_random)
							add_this = (randomGenerator.drawUniform(0.0,1.0)<=init_random->PROB_OBS);
					else	add_this = init_manual->visible[nKF*nUnknowns_k2f + nLM];

					if (add_this)
					{
						// Create observation: KF[nKF] -> LM[nLM]
						my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_df::col_t & dh_df_j = lin_system.dh_df.getCol(nLM);

						// Random is ok for this test:
						if (dh_dAp_i)
						{
							randomGenerator.drawGaussian1DMatrix( (*dh_dAp_i)[idx_obs].num  );
							(*dh_dAp_i)[idx_obs].sym.is_valid = &valid_true;
						}
						randomGenerator.drawGaussian1DMatrix( dh_df_j[idx_obs].num.setRandom() );
						dh_df_j[idx_obs].sym.is_valid = &valid_true;

						idx_obs++;
					}
				}
			}
			// Debug point:
			//if ( idx_obs != (1+nUnknowns_k2k)*nUnknowns_k2f ) cout << "#k2k: " << nUnknowns_k2k << " #k2f: " << nUnknowns_k2f << " #obs: " << idx_obs << " out of max.=" << (1+nUnknowns_k2k)*nUnknowns_k2f << endl;
		}

		// A default gradient:
		Eigen::VectorXd  minus_grad; // The negative of the gradient.
		const size_t idx_start_f = 6*nUnknowns_k2k;
		const size_t nLMs_scalars = 3*nUnknowns_k2f;
		const size_t nUnknowns_scalars = idx_start_f + nLMs_scalars;

		minus_grad.resize(nUnknowns_scalars);
		minus_grad.setOnes();

	#if 0
		lin_system.dh_dAp.saveToTextFileAsDense("test_dh_dAp.txt");
		lin_system.dh_df.saveToTextFileAsDense("test_dh_df.txt");
	#endif

		// ------------------------------------------------------------
		// 1st) Evaluate Schur naively with dense matrices
		// ------------------------------------------------------------
		CMatrixDouble  dense_dh_dAp, dense_dh_df;
		lin_system.dh_dAp.getAsDense(dense_dh_dAp);
		lin_system.dh_df.getAsDense (dense_dh_df);

		const CMatrixDouble  dense_HAp  = dense_dh_dAp.transpose() * dense_dh_dAp;
		const CMatrixDouble  dense_Hf   = dense_dh_df.transpose()  * dense_dh_df;
		const CMatrixDouble  dense_HApf = dense_dh_dAp.transpose() * dense_dh_df;

		CMatrixDouble  dense_Hf_plus_lambda = dense_Hf;
		for (size_t i=0;i<nLMs_scalars;i++)
			dense_Hf_plus_lambda(i,i)+=lambda;
//.........这里部分代码省略.........

//==============================================================================
void Problem::setOptimalSolution(const Eigen::VectorXd& _optParam)
{
  assert(static_cast<std::size_t>(_optParam.size()) == mDimension
         && "Invalid size.");
  mOptimalSolution = _optParam;
}

 inline void resize(Eigen::VectorXd & v,
                    std::size_t new_size)
 {
   v.resize(new_size);
 }

inline std::vector<double> eigenVectorToStdVector(const Eigen::VectorXd& v) {
    std::vector<double> m(v.size());
    for (int i = 0; i < v.size(); i++)
        m[i] = v[i];
    return m;
}

void multiTaskRecursiveLinearEstimator::setOutputErrorStandardDeviation(const Eigen::VectorXd &sigma_oe_input)
{
    assert(sigma_oe.size() == m);
    assert(sigma_oe_input.size() == m);
    sigma_oe = sigma_oe_input;
}

bool mrpt::vision::pnp::rpnp::compute_pose(
	Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
	// selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling
	int i1 = 0, i2 = 1;
	double lmin =
		Q(0, i1) * Q(0, i2) + Q(1, i1) * Q(1, i2) + Q(2, i1) * Q(2, i2);

	Eigen::MatrixXi rij(n, 2);

	R_ = Eigen::MatrixXd::Identity(3, 3);
	t_ = Eigen::Vector3d::Zero();

	for (int i = 0; i < n; i++)
		for (int j = 0; j < 2; j++) rij(i, j) = rand() % n;

	for (int ii = 0; ii < n; ii++)
	{
		int i = rij(ii, 0), j = rij(ii, 1);

		if (i == j) continue;

		double l = Q(0, i) * Q(0, j) + Q(1, i) * Q(1, j) + Q(2, i) * Q(2, j);

		if (l < lmin)
		{
			i1 = i;
			i2 = j;
			lmin = l;
		}
	}

	// calculating the rotation matrix of $O_aX_aY_aZ_a$.
	Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec;

	p1 = P.col(i1);
	p2 = P.col(i2);
	p0 = (p1 + p2) / 2;

	x = p2 - p0;
	x /= x.norm();

	if (std::abs(x(1)) < std::abs(x(2)))
	{
		dum_vec << 0, 1, 0;
		z = x.cross(dum_vec);
		z /= z.norm();
		y = z.cross(x);
		y /= y.norm();
	}
	else
	{
		dum_vec << 0, 0, 1;
		y = dum_vec.cross(x);
		y /= y.norm();
		z = x.cross(y);
		x /= x.norm();
	}

	Eigen::Matrix3d R0;

	R0.col(0) = x;
	R0.col(1) = y;
	R0.col(2) = z;

	for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0);

	// Dividing the n - point set into(n - 2) 3 - point subsets
	// and setting up the P3P equations

	Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2);
	double cg1 = v1.dot(v2);
	double sg1 = sqrt(1 - cg1 * cg1);
	double D1 = (P.col(i1) - P.col(i2)).norm();
	Eigen::MatrixXd D4(n - 2, 5);

	int j = 0;
	Eigen::Vector3d vi;
	Eigen::VectorXd rowvec(5);
	for (int i = 0; i < n; i++)
	{
		if (i == i1 || i == i2) continue;

		vi = Q.col(i);
		double cg2 = v1.dot(vi);
		double cg3 = v2.dot(vi);
		double sg2 = sqrt(1 - cg2 * cg2);
		double D2 = (P.col(i1) - P.col(i)).norm();
		double D3 = (P.col(i) - P.col(i2)).norm();

		// get the coefficients of the P3P equation from each subset.

		rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3);
		D4.row(j) = rowvec;
		j += 1;

		if (j > n - 3) break;
	}

	Eigen::VectorXd D7(8), dumvec(8), dumvec1(5);
//.........这里部分代码省略.........

Eigen::VectorXd RBM::operator()(const Eigen::VectorXd& x)
{
  v = x.transpose();
  sampleHgivenV();
  return ph.transpose();
}