void PoslvControl::messageRead( const poslv::VehicleNavigationSolutionMsgConstPtr& msg) { static bool firstVNS = true; static double latRef; static double longRef; static double altRef; if (firstVNS) { latRef = msg->latitude; longRef = msg->longitude; altRef = msg->altitude; firstVNS = false; } double x_ecef, y_ecef, z_ecef; Geo::wgs84ToEcef(msg->latitude, msg->longitude, msg->altitude, x_ecef, y_ecef, z_ecef); double x_enu, y_enu, z_enu; Geo::ecefToEnu(x_ecef, y_ecef, z_ecef, latRef, longRef, altRef, x_enu, y_enu, z_enu); Eigen::Vector3d orientation = Eigen::Vector3d(Utils::deg2rad(-msg->heading) + M_PI / 2.0, Utils::deg2rad(-msg->pitch), Utils::deg2rad(msg->roll)); _linearVelocity = Geo::R_ENU_NED::getInstance().getMatrix() * Eigen::Vector3d(msg->northVelocity, msg->eastVelocity, msg->downVelocity); _angularVelocity = Eigen::Vector3d(Utils::deg2rad(msg->angularRateLong), Utils::deg2rad(-msg->angularRateTrans), Utils::deg2rad(-msg->angularRateDown)); _acceleration = Eigen::Vector3d(msg->accLong, -msg->accTrans, -msg->accDown); _T_w_i = Eigen::Translation3d(x_enu, y_enu, z_enu) * Eigen::AngleAxisd(orientation(0), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(orientation(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(orientation(2), Eigen::Vector3d::UnitX()); emit poseUpdate(_T_w_i); static size_t updateCount = 0; updateCount++; if (updateCount >= _ui->rateSpinBox->value()) { _path += Points<double, 3>::Point(x_enu, y_enu, z_enu); emit updateViews(); updateCount = 0; } }
bool SolverSLAM2DLinear::solveOrientation() { assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph"); assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0"); VectorXD b, x; // will be used for theta and x/y update b.setZero(_optimizer->indexMapping().size()); x.setZero(_optimizer->indexMapping().size()); typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix; ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) blockIndeces[i] = i+1; SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size()); // building the structure, diagonal for each active vertex for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; int poseIdx = v->hessianIndex(); ScalarMatrix* m = H.block(poseIdx, poseIdx, true); m->setZero(); } HyperGraph::VertexSet fixedSet; // off diagonal for each edge for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { # ifndef NDEBUG EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it); assert(e && "Active edges contain non-odometry edge"); // # else EdgeSE2* e = static_cast<EdgeSE2*>(*it); # endif OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); int ind1 = from->hessianIndex(); int ind2 = to->hessianIndex(); if (ind1 == -1 || ind2 == -1) { if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices if (ind2 == -1) fixedSet.insert(to); continue; } bool transposedBlock = ind1 > ind2; if (transposedBlock){ // make sure, we allocate the upper triangle block std::swap(ind1, ind2); } ScalarMatrix* m = H.block(ind1, ind2, true); m->setZero(); } // walk along the Minimal Spanning Tree to compute the guess for the robot orientation assert(fixedSet.size() == 1); VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin()); VectorXD thetaGuess; thetaGuess.setZero(_optimizer->indexMapping().size()); UniformCostFunction uniformCost; HyperDijkstra hyperDijkstra(_optimizer); hyperDijkstra.shortestPaths(root, &uniformCost); HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap()); ThetaTreeAction thetaTreeAction(thetaGuess.data()); HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction); // construct for the orientation for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { EdgeSE2* e = static_cast<EdgeSE2*>(*it); VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]); VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]); double omega = e->information()(2,2); double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()]; double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()]; double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess); bool fromNotFixed = !(from->fixed()); bool toNotFixed = !(to->fixed()); if (fromNotFixed || toNotFixed) { double omega_r = - omega * error; if (fromNotFixed) { b(from->hessianIndex()) -= omega_r; (*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega; if (toNotFixed) { if (from->hessianIndex() > to->hessianIndex()) (*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega; else (*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega; } } if (toNotFixed ) { b(to->hessianIndex()) += omega_r; (*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega; } } } // solve orientation typedef LinearSolverCSparse<ScalarMatrix> SystemSolver; SystemSolver linearSystemSolver; linearSystemSolver.init(); bool ok = linearSystemSolver.solve(H, x.data(), b.data()); if (!ok) { cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl; return false; } // update the orientation of the 2D poses and set translation to 0, GN shall solve that root->setToOrigin(); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]); int poseIdx = v->hessianIndex(); SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx))); v->setEstimate(poseUpdate); } return true; }