本文整理汇总了C++中Plane3D::fromVector方法的典型用法代码示例。如果您正苦于以下问题：C++ Plane3D::fromVector方法的具体用法？C++ Plane3D::fromVector怎么用？C++ Plane3D::fromVector使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Plane3D的用法示例。
在下文中一共展示了Plane3D::fromVector方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
int main (int argc , char ** argv){
int maxIterations;
bool verbose;
bool robustKernel;
double lambdaInit;
CommandArgs arg;
bool fixSensor;
bool fixPlanes;
bool fixFirstPose;
bool fixTrajectory;
bool planarMotion;
bool listSolvers;
string strSolver;
cerr << "graph" << endl;
arg.param("i", maxIterations, 5, "perform n iterations");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("solver", strSolver, "lm_var", "select one specific solver");
arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
arg.param("robustKernel", robustKernel, false, "use robust error functions");
arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot");
arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory");
arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose");
arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)");
arg.param("planarMotion", planarMotion, false, "robot moves on a plane");
arg.param("listSolvers", listSolvers, false, "list the solvers");
arg.parseArgs(argc, argv);
SparseOptimizer* g=new SparseOptimizer();
ParameterSE3Offset* odomOffset=new ParameterSE3Offset();
odomOffset->setId(0);
g->addParameter(odomOffset);
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
OptimizationAlgorithmProperty solverProperty;
OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty);
g->setAlgorithm(solver);
if (listSolvers){
solverFactory->listSolvers(cerr);
return 0;
}
if (! g->solver()){
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
cerr << "available solvers: " << endl;
solverFactory->listSolvers(cerr);
cerr << "--------------" << endl;
return 0;
}
cerr << "sim" << endl;
Simulator* sim = new Simulator(g);
cerr << "robot" << endl;
Robot* r=new Robot(g);
cerr << "planeSensor" << endl;
Matrix3d R=Matrix3d::Identity();
R <<
0, 0, 1,
-1, 0, 0,
0, -1, 0;
Isometry3d sensorPose=Isometry3d::Identity();
sensorPose.matrix().block<3,3>(0,0) = R;
sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2);
PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose);
ps->_nplane << 0.03, 0.03, 0.005;
r->_sensors.push_back(ps);
sim->_robots.push_back(r);
cerr << "p1" << endl;
Plane3D plane;
PlaneItem* pi =new PlaneItem(g,1);
plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.));
pi =new PlaneItem(g,2);
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
cerr << "p2" << endl;
pi =new PlaneItem(g,3);
plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
Quaterniond q, iq;
if (planarMotion) {
r->_planarMotion = true;
r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025;
q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix());
//.........这里部分代码省略.........