initial commit for factor graph diff experiments with diff_factors

python_examples/FGraphDiff_2d.py

@@ -0,0 +1,21 @@
+#
+import mrob
+import numpy as np
+
+# simple example for FGraphDiff to solve
+graph = mrob.FGraphDiff()
+
+x = np.random.randn(3)
+n1 = graph.add_node_pose_2d(x)
+x = 1 + np.random.randn(3)*1e-1
+n2 = graph.add_node_pose_2d(x)
+print('node 1 id = ', n1, ' , node 2 id = ', n2)
+
+invCov = np.identity(3)
+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.LM)
+graph.print(True)
+
+

src/CMakeLists.txt

@@ -21,6 +21,9 @@ ADD_SUBDIRECTORY(geometry)
 INCLUDE_DIRECTORIES(FGraph)
 ADD_SUBDIRECTORY(FGraph)
 
+INCLUDE_DIRECTORIES(FGraphDiff)
+ADD_SUBDIRECTORY(FGraphDiff)
+
 INCLUDE_DIRECTORIES(PCRegistration)
 ADD_SUBDIRECTORY(PCRegistration)
 

src/FGraphDiff/CMakeLists.txt

@@ -0,0 +1,40 @@
+# locate the necessary dependencies, if any
+
+project(FGraphDiff)
+
+# extra header files
+SET(headers
+    ../FGraph/mrob/node.hpp
+    mrob/factor_diff.hpp
+    mrob/factor_graph_diff.hpp
+    mrob/factor_graph_diff_solve.hpp
+)
+
+# extra source files
+SET(sources
+    ../FGraph/node.cpp
+    factor_diff.cpp
+    factor_graph_diff.cpp
+    factor_graph_diff_solve.cpp
+)
+
+SET(factors_headers
+    mrob/factors/factor1Pose2d_diff.hpp
+    mrob/factors/factor2Poses2d_diff.hpp
+)
+
+SET(factors_sources
+    factors/factor1Pose2d_diff.cpp
+    factors/factor2Poses2d_diff.cpp
+)
+
+# create library
+ADD_LIBRARY(FGraphDiff ${sources} ${factors_sources})
+TARGET_LINK_LIBRARIES(FGraphDiff SE3 common FGraph)
+
+SET_TARGET_PROPERTIES(FGraphDiff PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY "${MROB_LIB_DIR}"
+)
+
+ADD_SUBDIRECTORY(examples)
+#ADD_SUBDIRECTORY(tests)

src/FGraphDiff/examples/CMakeLists.txt

@@ -0,0 +1,2 @@
+ADD_EXECUTABLE(example_FGraphDiff_2d_example  example_solver_2d.cpp)
+TARGET_LINK_LIBRARIES(example_FGraphDiff_2d_example FGraphDiff)

src/FGraphDiff/examples/example_solver_2d.cpp

@@ -0,0 +1,81 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * example_solver.cpp
+ *
+ *  Created on: April 10, 2019
+ *      Author: Gonzalo Ferrer
+ *              [email protected]
+ *              Mobile Robotics Lab, Skoltech
+ */
+
+
+
+
+
+#include "mrob/factor_graph_diff_solve.hpp"
+#include "mrob/factors/factor1Pose2d_diff.hpp"
+#include "mrob/factors/factor2Poses2d_diff.hpp"
+#include "mrob/factors/nodePose2d.hpp"
+
+
+#include <iostream>
+
+int main ()
+{
+
+    // create a simple graph to solve:
+    //     anchor ------ X1 ------- obs ---------- X2
+    mrob::FGraphDiffSolve graph(mrob::FGraphDiffSolve::ADJ);
+
+    // Initial node is defined at 0,0,0, and anchor factor actually observing it at 0
+    mrob::Mat31 x, obs;
+    x = mrob::Mat31::Random()*0.1;
+    obs = mrob::Mat31::Zero();
+    // Nodes and factors are added to the graph using polymorphism. That is why
+    // we need to declare here what specific kind of nodes or factors we use
+    // while the definition is an abstract class (Node or DiffFactor)
+    std::shared_ptr<mrob::Node> n1(new mrob::NodePose2d(x));
+    graph.add_node(n1);
+    Mat3 obsInformation= Mat3::Identity();
+    std::shared_ptr<mrob::DiffFactor> f1(new mrob::Factor1Pose2d_diff(obs,n1,obsInformation*1e6));
+    graph.add_factor(f1);
+
+    // Node 2, initialized at 0,0,0
+    if (0){
+    std::shared_ptr<mrob::Node> n2(new mrob::NodePose2d(x));
+    graph.add_node(n2);
+
+    // Add odom factor = [drot1, dtrans, drot2]
+    obs << 0, 1, 0;
+    //obs << M_PI_2, 0.5, 0;
+    // this factor assumes that the current value of n2 (node destination) is updated according to obs
+    std::shared_ptr<mrob::DiffFactor> f2(new mrob::Factor2Poses2dOdom_diff(obs,n1,n2,obsInformation));
+    graph.add_factor(f2);
+
+    obs << -1 , -1 , 0;
+    std::shared_ptr<mrob::DiffFactor> f3(new mrob::Factor2Poses2d_diff(obs,n2,n1,obsInformation));
+    graph.add_factor(f3);
+    }
+
+    // solve the Gauss Newton optimization
+    graph.print(true);
+    graph.solve(mrob::FGraphDiffSolve::LM);
+
+    std::cout << "\nSolved, chi2 = " << graph.chi2() << std::endl;
+
+    graph.print(true);
+    return 0;
+}

src/FGraphDiff/factor_diff.cpp

@@ -0,0 +1,31 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * factor.cpp
+ *
+ *  Created on: Feb 27, 2018
+ *      Author: Gonzalo Ferrer
+ *              [email protected]
+ *              Mobile Robotics Lab, Skoltech 
+ */
+
+#include "mrob/factor_diff.hpp"
+
+using namespace mrob;
+
+DiffFactor::DiffFactor(uint_t dim, uint_t allNodesDim, robustFactorType factor_type, uint_t potNumberNodes): 
+    Factor(dim, allNodesDim,factor_type,potNumberNodes){}
+
+DiffFactor::~DiffFactor(){}

src/FGraphDiff/factor_graph_diff.cpp

@@ -0,0 +1,53 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * factor_graph.cpp
+ *
+ *  Created on: Feb 12, 2018
+ *      Author: Gonzalo Ferrer
+ *              [email protected]
+ *              Mobile Robotics Lab, Skoltech 
+ */
+
+#include <iostream>
+#include <mrob/factor_graph_diff.hpp>
+
+
+using namespace mrob;
+
+FGraphDiff::FGraphDiff()
+{}
+
+FGraphDiff::~FGraphDiff()
+{
+    factors_.clear();
+    nodes_.clear();
+    eigen_factors_.clear();
+}
+
+factor_id_t FGraphDiff::add_factor(std::shared_ptr<DiffFactor> &factor)
+{
+    factor->set_id(factors_.size());
+    factors_.emplace_back(factor);
+    obsDim_ += factor->get_dim_obs();
+    return factor->get_id();
+}
+
+factor_id_t FGraphDiff::add_eigen_factor(std::shared_ptr<DiffEigenFactor> &factor)
+{
+    factor->set_id(eigen_factors_.size());
+    eigen_factors_.emplace_back(factor);
+    return factor->get_id();
+}