diff --git a/mrobpy/FGraphPy.cpp b/mrobpy/FGraphPy.cpp index fa749c91..63e67a45 100644 --- a/mrobpy/FGraphPy.cpp +++ b/mrobpy/FGraphPy.cpp @@ -230,11 +230,18 @@ void init_FGraph(py::module &m) .def("add_factor_2poses_2d", &FGraphPy::add_factor_2poses_2d, "Factors connecting 2 poses. If last input set to true (by default false), also updates the value of the target Node according to the new obs + origin node", py::arg("obs"), - py::arg("nodeOridingId"), + py::arg("nodeOriginId"), py::arg("nodeTargetId"), py::arg("obsInvCov"), py::arg("updateNodeTarget") = false) - .def("add_factor_2poses_2d_odom", &FGraphPy::add_factor_2poses_2d_odom) + .def("add_factor_2poses_2d_odom", &FGraphPy::add_factor_2poses_2d_odom, + "add_factor_2poses_2d_odom(obs, nodeOriginId, nodeTargetId, W)" + "\nFactor connecting 2 poses, following an odometry model." + "\nArguments are obs, nodeOriginId, nodeTargetId and obsInvCov", + py::arg("obs"), + py::arg("nodeOriginId"), + py::arg("nodeTargetId"), + py::arg("obsInvCov")) // 2d Landmkarks .def("add_node_landmark_2d", &FGraphPy::add_node_landmark_2d, "Ladmarks are 2D points, in [x,y]") diff --git a/python_examples/FGraph_2d.py b/python_examples/FGraph_2d.py index 9234c8e9..8097b99b 100644 --- a/python_examples/FGraph_2d.py +++ b/python_examples/FGraph_2d.py @@ -16,10 +16,10 @@ invCov = np.identity(3) -graph.add_factor_1pose_2d(np.zeros(3),n1,1e6*invCov) +graph.add_factor_1pose_2d(np.array([0,0,np.pi/4]),n1,1e6*invCov) graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov) -graph.solve(mrob.GN) +graph.solve(mrob.LM) graph.print(True) diff --git a/python_examples/FGraph_landmark_2d_example.py b/python_examples/FGraph_landmark_2d_example.py index 0fd32d33..0a25fe49 100644 --- a/python_examples/FGraph_landmark_2d_example.py +++ b/python_examples/FGraph_landmark_2d_example.py @@ -7,10 +7,11 @@ graph = mrob.FGraph() #initial point at [0,0,0] with some noise -n1 = graph.add_node_pose_2d(np.random.randn(3)*0.05) -# non-initialized landmarks, but they could be initialiazed +n1 = graph.add_node_pose_2d(np.random.randn(3)*0.0005) +# non-initialized landmarks, but they could be initialiazed when adding factors +# In case of 2d landmarks, they might be sensitive to IC, around 0 of initiakl pose and landmark On those cases might (rearely) GN fail to converge. l1 = graph.add_node_landmark_2d(np.array([0,0])) -l2 = graph.add_node_landmark_2d(np.zeros(2)) +l2 = graph.add_node_landmark_2d(np.array([0,0])) print('node pose id = ', n1, ' , node landmark 1 id = ', l1 , ' , node landmark 2 id = ', l2) # anchor factor @@ -23,14 +24,17 @@ graph.add_factor_1pose_1landmark_2d(obs,n1,l1,W) obs = np.array([1,np.pi/2]) graph.add_factor_1pose_1landmark_2d(obs,n1,l2,W) -graph.print(True) print('\n\n\n Solving Fgraph:\n') -#graph.solve(mrob.fgraph.GN) #1 iteration of Gauss-Newton -graph.solve(mrob.LM) #as many iterations until convergence for Levenberg Marquardt -graph.print(True) +graph.solve(mrob.GN) #1 iteration of Gauss-Newton +graph.solve(mrob.GN) +graph.solve(mrob.GN) +graph.solve(mrob.GN) +#graph.solve(mrob.LM) +#graph.print(True) +print(graph.get_estimated_state()) print('Current chi2 = ', graph.chi2() ) # re-evaluates the error, on print it is only the error on evalation before update diff --git a/python_examples/FGraph_point_plane_align.py b/python_examples/FGraph_point_plane_align.py index 32592598..581b79e5 100644 --- a/python_examples/FGraph_point_plane_align.py +++ b/python_examples/FGraph_point_plane_align.py @@ -85,3 +85,11 @@ def vis_arr(X): print('Error in poses rotation= ', T.distance_rotation(Testimated), ', distance trans = ', T.distance_trans(Testimated)) vis_her(X,Y,Testimated.T()) + + +L = graph.get_information_matrix().todense() +D, U = np.linalg.eig(L) +print(D) +print(U) + + diff --git a/src/FGraph/factors/factor1Pose1Landmark2d.cpp b/src/FGraph/factors/factor1Pose1Landmark2d.cpp index 15703a6e..46eff919 100644 --- a/src/FGraph/factors/factor1Pose1Landmark2d.cpp +++ b/src/FGraph/factors/factor1Pose1Landmark2d.cpp @@ -52,7 +52,11 @@ Factor1Pose1Landmark2d::Factor1Pose1Landmark2d(const Mat21 &observation, std::sh if (initializeLandmark) { - // TODO Initialize landmark value to whatever observation we see from current pose + Mat31 x = nodePose->get_state(); + Mat21 dl; + dl << std::cos(obs_(1)+x(2)), std::sin(obs_(1)+x(2)); + Mat21 land = x.head(2) + obs_(0)*dl; + nodeLandmark->set_state(land); } } @@ -71,17 +75,16 @@ void Factor1Pose1Landmark2d::evaluate_residuals() poseIndex = 1; } // From the local frame we observe the landmark - // TODO LM needs too many iterations, check once again gradients state_ = get_neighbour_nodes()->at(poseIndex)->get_state(); landmark_ = get_neighbour_nodes()->at(landmarkIndex)->get_state(); dx_ = landmark_(0) - state_(0); dy_ = landmark_(1) - state_(1); q_ = dx_*dx_ + dy_*dy_; + // r = [range, bearing] r_ << std::sqrt(q_), std::atan2(dy_,dx_) - state_(2); - r_ -= obs_; - r_(2) = wrap_angle(r_(2)); - + r_ = r_-obs_; + r_(1) = wrap_angle(r_(1)); } void Factor1Pose1Landmark2d::evaluate_jacobians() { diff --git a/src/FGraph/factors/factor1Pose2d.cpp b/src/FGraph/factors/factor1Pose2d.cpp index 918902d7..536ff581 100644 --- a/src/FGraph/factors/factor1Pose2d.cpp +++ b/src/FGraph/factors/factor1Pose2d.cpp @@ -43,6 +43,7 @@ void Factor1Pose2d::evaluate_jacobians() void Factor1Pose2d::evaluate_residuals() { r_ = get_neighbour_nodes()->at(0).get()->get_state() - obs_; + r_(2) = wrap_angle(r_(2)); } void Factor1Pose2d::evaluate_chi2() diff --git a/src/FGraph/factors/factor2Poses2d.cpp b/src/FGraph/factors/factor2Poses2d.cpp index 9fe7b8da..69a82312 100644 --- a/src/FGraph/factors/factor2Poses2d.cpp +++ b/src/FGraph/factors/factor2Poses2d.cpp @@ -62,8 +62,8 @@ void Factor2Poses2d::evaluate_residuals() { // r = h(i,j) - obs = Ri^T * (xj- xi) - obs . From "i", i.e, at its reference frame, we observe xj Mat31 h = nodeTarget - nodeOrigin; Mat2 RiT; - double c1 = cos(nodeOrigin(2)), - s1 = sin(nodeOrigin(2)); + double c1 = std::cos(nodeOrigin(2)), + s1 = std::sin(nodeOrigin(2)); RiT << c1, s1, -s1, c1; h.head(2) = RiT * h.head(2); @@ -79,8 +79,8 @@ void Factor2Poses2d::evaluate_jacobians() // r = Ri^T * (xj- xi) - obs // J1 = [-R1^T, J2 = [R1^T 0] // [ ] [0 1] - double c1 = cos(nodeOrigin(2)), - s1 = sin(nodeOrigin(2)), + double c1 = std::cos(nodeOrigin(2)), + s1 = std::sin(nodeOrigin(2)), dx = nodeTarget(0) - nodeOrigin(0), dy = nodeTarget(1) - nodeOrigin(1); J_ << -c1, -s1, -s1*dx + c1*dy, c1, s1, 0, @@ -125,7 +125,7 @@ void Factor2Poses2dOdom::evaluate_residuals() auto prediction = get_odometry_prediction(stateOrigin, obs_); r_ = prediction - stateTarget; - r_[2] = wrap_angle(r_[2]); + r_(2) = wrap_angle(r_(2)); } void Factor2Poses2dOdom::evaluate_jacobians() @@ -133,7 +133,7 @@ void Factor2Poses2dOdom::evaluate_jacobians() // Get the position of node we are traversing from Mat31 node1 = get_neighbour_nodes()->at(0).get()->get_state(); - auto s = -obs_[1] * sin(node1[2]), c = obs_[1] * sin(node1[2]); + auto s = -obs_(1) * std::sin(node1(2)), c = obs_(1) * std::cos(node1(2)); // Jacobians for odometry model which are: G and -I J_ << 1, 0, s, -1, 0, 0, @@ -143,10 +143,10 @@ void Factor2Poses2dOdom::evaluate_jacobians() Mat31 Factor2Poses2dOdom::get_odometry_prediction(Mat31 state, Mat31 motion) { - state[2] += motion[0]; - state[0] += motion[1] * cos(state[2]); - state[1] += motion[1] * sin(state[2]); - state[2] += motion[2]; + state(2) += motion(0); + state(0) += motion(1) * std::cos(state(2)); + state(1) += motion(1) * std::sin(state(2)); + state(2) += motion(2); return state; } diff --git a/src/FGraph/factors/nodeLandmark2d.cpp b/src/FGraph/factors/nodeLandmark2d.cpp index bb6fde7b..8cfc715a 100644 --- a/src/FGraph/factors/nodeLandmark2d.cpp +++ b/src/FGraph/factors/nodeLandmark2d.cpp @@ -43,21 +43,16 @@ NodeLandmark2d::~NodeLandmark2d() void NodeLandmark2d::update(const Eigen::Ref &dx) { - //TODO test cast - Mat21 dxf = dx; - state_ += dxf; + state_ += dx; } void NodeLandmark2d::update_from_auxiliary(const Eigen::Ref &dx) { - //TODO test cast - Mat21 dxf = dx; - state_ = auxiliaryState_ + dxf; + state_ = auxiliaryState_ + dx; } void NodeLandmark2d::set_state(const Eigen::Ref &x) { - // cast is done by Eigen state_ = x; } diff --git a/src/FGraph/mrob/factor.hpp b/src/FGraph/mrob/factor.hpp index 98c6cabf..0df19864 100644 --- a/src/FGraph/mrob/factor.hpp +++ b/src/FGraph/mrob/factor.hpp @@ -101,7 +101,7 @@ class Factor{ * Return a Ref to a dynamic matrix, while the child matrix should declare * all these variables as fixed size matrices, and ref takes care of * doing the conversion with minimal temporary artifacts - * Observation can be a 3d point, a 3d pose (transfromation 4x4), etc. + * Observation can be a 3d point, a 3d pose (transformation 4x4), etc. */ virtual const Eigen::Ref get_obs() const = 0; /** diff --git a/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp b/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp index 96dca51c..6d3a7314 100644 --- a/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp +++ b/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp @@ -49,9 +49,6 @@ namespace mrob{ * z = [range,bearing] is a 2d vector, composed of a range and bearing in the local frame. * l is a 2d point encoding the landmark position l = [x,y] * - * and the residual is thus: - * r = T-1*l - z - * * * Constructor functions will be overloaded to include the pointers of the nodes, * The convention is 2d pose, we observe node destination,