├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── README.md ├── cmake └── cotire.cmake ├── include ├── gps_test.h └── test_common.h ├── lib ├── factors │ ├── include │ │ └── factors │ │ │ ├── SE3.h │ │ │ ├── attitude_3d.h │ │ │ ├── camera.h │ │ │ ├── carrier_phase.h │ │ │ ├── clock_bias_dynamics.h │ │ │ ├── clock_dynamics.h │ │ │ ├── dynamics_1d.h │ │ │ ├── dynamics_3d.h │ │ │ ├── imu_1d.h │ │ │ ├── imu_3d.h │ │ │ ├── position_1d.h │ │ │ ├── position_3d.h │ │ │ ├── pseudorange.h │ │ │ ├── range_1d.h │ │ │ ├── switch_pseudorange.h │ │ │ └── transform_1d.h │ └── src │ │ └── imu_3d.cpp └── utils │ ├── include │ └── utils │ │ ├── jac.h │ │ ├── logger.h │ │ ├── robot1d.h │ │ └── trajectory_sim.h │ └── src │ ├── jac.cpp │ ├── robot1d.cpp │ └── trajectory_sim.cpp ├── matlab ├── Imu3DCheckPropagation.m ├── Imu3DMultiWindowPlot.m ├── TimeOffsetMultiWindowPlot.m ├── TrajectoryOptimization1DMultiPlot.m ├── TrajectoryOptimization1DSinglePlot.m └── TrajectoryOptimization3DSinglePlot.m ├── python ├── CarrierPhase.ImuTrajectory.py ├── CarrierPhase.Trajectory.py ├── Imu3D_MultiWindow.py ├── Imu3D_Propagation.py ├── MultipathPseudorange.StandardResidual.py ├── MultipathPseudorange.SwitchedResidual.py ├── Pseudorange.ImuTrajectory.py ├── Pseudorange.Trajectory.py ├── Pseudorange.TrajectoryClockDynamics.py ├── TimeOffset.3DmultirotorPoseGraph.py ├── plotRawGPS.py ├── plotWindow.py └── plotWindow.pyc ├── scripts ├── build_and_run.sh └── jac_test.py └── src ├── attitude.cpp ├── camera.cpp ├── carrier_phase.cpp ├── control.cpp ├── dev_test.cpp ├── imu1d.cpp ├── imu3d.cpp ├── pose.cpp ├── position1d.cpp ├── position3d.cpp ├── pseudorange.cpp ├── switch.cpp ├── test_common.cpp └── time_offset.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | build/ 3 | *.idea/ 4 | *.html 5 | *.m~ 6 | *.swp 7 | gh-md-toc 8 | logs/ 9 | *.pyc 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/multirotor_sim"] 2 | path = lib/multirotor_sim 3 | url = git@github.com:superjax/multirotor_sim.git 4 | [submodule "lib/geometry"] 5 | path = lib/geometry 6 | url = git@github.com:superjax/geometry.git 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ceres_sandbox) 3 | 4 | set (CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") 5 | set(CMAKE_CXX_STANDARD 14) 6 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfma") 7 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx2 -mfma") 8 | 9 | include(cotire) 10 | find_package(Ceres REQUIRED) 11 | find_package(Eigen3 REQUIRED) 12 | find_package(GTest REQUIRED) 13 | 14 | add_subdirectory(lib/geometry) 15 | add_subdirectory(lib/multirotor_sim) 16 | 17 | include_directories(include 18 | lib 19 | lib/utils/include 20 | lib/factors/include 21 | ${EIGEN3_INCLUDE_DIRS}) 22 | 23 | add_library(utils 24 | lib/utils/src/jac.cpp 25 | lib/utils/src/robot1d.cpp 26 | lib/utils/src/trajectory_sim.cpp 27 | src/test_common.cpp) 28 | target_link_libraries(utils multirotor_sim) 29 | cotire(utils) 30 | 31 | set(TARGETS utils) 32 | 33 | function(make_gtest test_name source_file) 34 | add_executable(${test_name} ${source_file}) 35 | target_link_libraries(${test_name} 36 | utils 37 | geometry 38 | gtest 39 | gtest_main 40 | ${MULTIROTOR_SIM_LIBS} 41 | ${GTEST_LIBRARIES} 42 | ${CERES_LIBRARIES} 43 | ) 44 | cotire(${test_name}) 45 | endfunction() 46 | 47 | make_gtest(test_position1d src/position1d.cpp) 48 | make_gtest(test_position3d src/position3d.cpp) 49 | make_gtest(test_attitude src/attitude.cpp) 50 | make_gtest(test_pose src/pose.cpp) 51 | make_gtest(test_imu1d src/imu1d.cpp) 52 | make_gtest(test_imu3d src/imu3d.cpp) 53 | make_gtest(test_time_offset src/time_offset.cpp) 54 | make_gtest(test_camera src/camera.cpp) 55 | make_gtest(test_pseudorange src/pseudorange.cpp) 56 | make_gtest(test_control src/control.cpp) 57 | make_gtest(test_switch src/switch.cpp) 58 | make_gtest(test_carrier_phase src/carrier_phase.cpp) 59 | 60 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Ceres Solver Sandbox 2 | 3 | I wanted to learn the [ceres solver](http://ceres-solver.org/). There are a lot of great tutorials on their wiki, but I felt like I needed some practical experience with some simple problems before I jumped into full-blown SLAM. 4 | 5 | So, I broke the problem into little peices and wrote a bunch of simple cases to explore the capabilities of Ceres. Each section is written as a series of unit tests in the [gtest](https://github.com/google/googletest) framework. 6 | 7 | Borrowing off the _factor graph_ mindset (I'm from a SLAM background), I organized all my cost functions into _factors_, found in `include/factors` 8 | 9 | * [Position1D](#position1d) 10 | * [Position1D.AveragePoints](#position1daveragepoints) 11 | * [Position1D.AveragePointsWithParameterBlock](#position1daveragepointswithparameterblock) 12 | * [Robot1D.SLAM](#robot1dslam) 13 | * [Position3D](#position3d) 14 | * [Position3D.AveragePoints](#position3daveragepoints) 15 | * [Attitude3D](#attitude3d) 16 | * [Attitude3d.Check*](#attitude3dcheck) 17 | * [Attitude3d.AverageAttitude](#attitude3daverageattitude) 18 | * [Attitude3d.AverageAttitudeAutoDiff](#attitude3daverageattitudeautodiff) 19 | * [Pose3D](#pose3d) 20 | * [Pose3D.AveragePoseAutoDiff](#pose3daverageposeautodiff) 21 | * [Pose3D.GraphSLAM](#pose3dgraphslam) 22 | * [Imu1D](#imu1d) 23 | * [IMU.1DRobotSingleWindow](#imu1drobotsinglewindow) 24 | * [IMU.1DRobotLocalization](#imu1drobotlocalization) 25 | * [IMU.1DRobotSLAM](#imu1drobotslam) 26 | * [IMU.dydb](#imudydb) 27 | * [Imu3D](#imu3d) 28 | * [Imu3D.CheckDynamicsJacobians](#imu3dcheckdynamicsjacobians) 29 | * [Imu3D.CheckBiasJacobians](#imu3dcheckbiasjacobians) 30 | * [Imu3D.SingleWindow](#imu3dsinglewindow) 31 | * [Imu3D/MultiWindow](#imu3dmultiwindow) 32 | * [TimeOffset](#timeoffset) 33 | * [TimeOffset.1DRobotSLAM](#timeoffset1drobotslam) 34 | * [TimeOffset.3DMultirotorPoseGraph](#timeoffset3dmultirotorposegraph) 35 | * [IMU.3DRobotSLAM](#imu3drobotslam) 36 | * [Camera](#cameracpp) 37 | * [Camera.Intrinsics_Calibration](#cameraintrinsics_calibration) 38 | 39 | 40 | # Position1D 41 | The first set of unit tests looks at the simplest problems. 42 | 43 | ## Position1D.AveragePoints 44 | This finds the average of a bunch of 1D samples. It's a little like opening a piggy bank with a jackhammer, but we gotta start somewhere. Uses the `Position1DFactor` with analytical (trivial) jacobians. 45 | 46 | ## Position1D.AveragePointsWithParameterBlock 47 | This is exactly like `Position1D.AveragePoints`, but it adds a (redundant) parameter block. Just to test how `problem.AddParameterBlock` works. 48 | 49 | ## Robot1D.SLAM 50 | This performs 1D SLAM (haha!). There is a 1D robot which takes a 1m step along the real line, 7 times. Each step, he gets a (noisy) range measurement to 3 landmarks (also on the real line). It's a pretty trivial problem, but I wanted to explore how to handle multiple factors, and handle measurement variance in my factors. 51 | 52 | Uses `Transform1d` - the transform between each step with associated variance and `Range1dFactor` - the range to a landmark and associated variance. 53 | 54 | 55 | # Position3D 56 | ## Position3D.AveragePoints 57 | This finds the average of a bunch of 1D samples. Again, overkill, but uses the `Position3DFactor` with analytical jacobians. I also used this example to explore how Eigen interacts with Ceres. 58 | 59 | 60 | # Attitude3D 61 | The second set of unit tests looks at attitude, and the `LocalParameterization` functionality in ceres. 62 | 63 | ## Attitude3d.Check* 64 | In writing the `QuatFactor` and `QuatParameterization` classes, I wanted to make sure that the associated `Plus`, `Evaluate` and `ComputeJacobian` functions were correct. These were pretty straight-forward, except the jacobian of the `QuatFactor`. That was terrible. Anyway, these are good for sanity checking the operations being performed by the factor and local parameterization. 65 | 66 | ## Attitude3d.AverageAttitude 67 | This takes a quaternion and creates 1000 samples normally distributed about this quaternion (using the tangent space to come up with the samples). Then, I use ceres solver to recover the mean. Probably the stupidest way to take a mean of a data set, but it exercises the use of a local parameterization, and non-trivial analytic jacobians over my factor. 68 | 69 | ## Attitude3d.AverageAttitudeAutoDiff 70 | Finds the average attitude of a sample of 1000 attitude measurements using the autodiff functionality for both the localparameterization and cost function. I also used my templated `Quat` library and let Ceres auto-diff through my library (based on Eigen). 71 | 72 | This example led to an interesting result. I spent something like 6 hours deriving the analytical jacobian for the previous example, and found essentially _zero_ performance increase (In fact, the auto-diff version is typically a little bit faster). While I feel a little sheepish for thinking that I could out-optimize the compiler on this once, fewer jacobians over manifolds for me is a good thing. After this result I pretty much stopped using analytical jacobians at all and let the auto-differentiation engine do its magic. 73 | 74 | # Pose3D 75 | Next, I figured I could do some pose-graph SLAM in SE3 76 | 77 | ## Pose3D.AveragePoseAutoDiff 78 | As before, just to exercise auto differentiation over a tangent space, I used ceres to find the average of a set of 1000 random `Xform` samples. (using my templated `Xform` library to represent members of homogeneous transforms) 79 | 80 | ## Pose3D.GraphSLAM 81 | This is the most basic kind of non-trivial SLAM. We have a bunch of nodes, and edges between nodes. We also have loop-closures so that the graph is over-constrained and require optimization to find the maximum-likelihood configuration of all the nodes and edges. 82 | 83 | I'm using full 6DOF edges and nodes in this graph and the first node is fixed at the origin. 84 | 85 | I'm also using auto-differentiated Factors and Local Parameterizations 86 | 87 | # Imu1D 88 | Next, I wanted to look into estimating motion of a robot using IMU inputs. The following examples use a simulated robot that moves at a constant rate along the real line, and but has a noisy IMU with a constant bias. 89 | 90 | ## IMU.1DRobotSingleWindow 91 | This example looks at a single preintegration window with the origin pose set constant and a direct measurement of the second pose. Basically, this shows that the unknown IMU bias can be inferred using my `Imu1DFactorAutoDiff` factor. 92 | 93 | ## IMU.1DRobotLocalization 94 | This example looks at 100 preintegration intervals with with a common constant unknown bias. The origin is set constant, and the final pose is given a very strong position and velocity measurement. The IMU bias is inferred using my `IMU1DFactorAutoDiff` factor. 95 | 96 | ## IMU.1DRobotSLAM 97 | Using the `Range1dFactor` with range measurements to several landmarks also on the real line and the `IMU1dFactor` to perform 1D SLAM. 98 | 99 | ## IMU.dydb 100 | In writing the `IMU1dFactor`, I had to figure out the jacobian to map changes in bias to changes in the measurement. This test proves that the jacobian I cam up with is right. 101 | 102 | # Imu3D 103 | The biggest reason I did all this was to help me in my work on SLAM with autonomous agents. Often, I have IMU measurements onboard, but these occur at a very high rate. The goal of these examples is to preintegrate these IMU measurements and estimate biases while doing SLAM. 104 | 105 | ## Imu3D.CheckDynamicsJacobians 106 | This examples simply checks the jacobians of the dynamics in the `Imu3DFactorCostFunction` class 107 | 108 | ## Imu3D.CheckBiasJacobians 109 | This example checks the jacobian used to modify the preintegrated measurement given a change in bias. 110 | 111 | ## Imu3D.SingleWindow 112 | This preintegrates a single window and estimates the IMU bias given a fixed origin and a measurement of the final pose. 113 | 114 | ## Imu3D/MultiWindow 115 | This example simulates the flight of a multirotor given measurements of pose at regular intervals. IMU is preintegrated at 250Hz while Pose measurements are supplied at 5Hz. The constant IMU biases are estimated over the interval. The results of this study can be visualized using the `Imu3DMultiWindowPlot.m` matlab script. 116 | 117 | # TimeOffset 118 | Another problem in Robotics is time synchronization between different sensors. The following examples show how to estimate this time offset. 119 | 120 | ## TimeOffset.1DRobotSLAM 121 | This example is a 1D robot performing SLAM with IMU preintegration as in the `IMU.1DRobotSLAM` example, except that I also have a measurement of the position of each node with a small time delay. I use the `Position1dFactorWithTimeOffset` factor to estimate this delay. 122 | 123 | ## TimeOffset.3DMultirotorPoseGraph 124 | This example is of a multirotor flying waypoints with a lagged position and attitude measurement (as experienced by a motion capture system). The goal is to estimate this offset, IMU bias and the location of all poses simultaneously. The results of this example can be visualized by running the `TimeOffsetMultiWindowPlot.m` matlab script. 125 | 126 | ## IMU.3DRobotSLAM 127 | This example is the full SLAM problem - a 3D rigid body moves in space, and has bearing measurements to several landmarks. Performs SLAM while inferring IMU biases. 128 | 129 | # Camera 130 | Next, I wanted to use the ceres solver to deal with the projection associated with a pinhole camera model. 131 | 132 | ## Camera.Intrinsics_Calibration 133 | This example simulates the calibration of a pinhole camera. A 3D rigid body gets simulated pixel measurements to known landmarks in the camera FOV. The camera intrinsics are estimated. 134 | 135 | # Control 136 | Finally, I wanted to see how well-suited Ceres was to solving the model-predictive control and trajectory optimization problem. Spoiler: It's not. 137 | 138 | ## Control.Robot1d_OptimizeTrajectorySingleWindow 139 | This implements the simple minimum force "move block" example from Matthew Kelly's [Trajectory Optimization tutorial paper](https://epubs.siam.org/doi/pdf/10.1137/16M1062569). The results of the optimization can be plotted with `TrajectoryOptimization1DPlot.m` 140 | 141 | ## Control.3DMultirotor_OptimizeTrajectory 142 | This extends the trajectory optimiztion problem to 3D with a quadrotor agent. This is trajectory optimization on manifold. This doesn't work very well, because the search space is so large that the lack of dynamics constraint results in a multitude of local minima. 143 | 144 | 145 | # Building The Code 146 | There aren't any real linux-specific dependencies that I know of, but I use linux almost exclusively and do not know if this will work on Windows or Mac. You do need Eigen `sudo apt install libeigen3-dev` and the ceres sovler, [installation](http://ceres-solver.org/installation.html). I would recommend building the latetest stable release from source and be sure to grab the suitesparse and cxsparse dependencies. This development took place with ceres 1.14.0 and eigen 3.3.4. I noticed that Eigen threw alignment errors with the `master` branch, which I found odd. You'll also need gtest [installation](https://www.eriksmistad.no/getting-started-with-google-test-on-ubuntu/). 147 | 148 | This project depends on my templated `geometry` library for homogeneous transforms and quaternions as well as my C++ multirotor simulator I developed with @jerelbn. (These are included as submodules). 149 | 150 | Once you've got all the libraries installed, it's a simple CMake build. 151 | 152 | ``` bash 153 | mkdir build 154 | cd build 155 | cmake .. -DCMAKE_BUILD_TYPE=Release 156 | make -j -l 157 | ``` 158 | 159 | # Running The Examples 160 | All the code in this repo is organized into a big unit test suite. The tests usuall all pass, but not always, and if they don't they are really close. Just run the `ceres_tests` executable. Being a Gtest suite, it has a number of command-line arguments that it supports. Try `-h` to see the available command-line options. 161 | 162 | ``` 163 | ./ceres_tests 164 | ``` -------------------------------------------------------------------------------- /include/gps_test.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "geometry/xform.h" 10 | #include "multirotor_sim/simulator.h" 11 | #include "multirotor_sim/controller.h" 12 | #include "multirotor_sim/satellite.h" 13 | #include "utils/estimator_wrapper.h" 14 | #include "utils/logger.h" 15 | #include "test_common.h" 16 | 17 | #include "factors/SE3.h" 18 | #include "factors/imu_3d.h" 19 | #include "factors/switch.h" 20 | #include "factors/pseudorange.h" 21 | #include "factors/clock_bias_dynamics.h" 22 | 23 | 24 | using namespace multirotor_sim; 25 | using namespace Eigen; 26 | using namespace xform; 27 | 28 | template 29 | class TestSwitchPRangeTraj : public EstimatorBase 30 | { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | TestSwitchPRangeTraj() : 34 | sim(false, 2) 35 | { 36 | init(); 37 | init_residual_blocks(); 38 | init_estimator(); 39 | } 40 | 41 | int n = 0; 42 | 43 | Simulator sim; 44 | Eigen::Matrix xhat0, xhat, x; 45 | Eigen::Matrix vhat0, vhat, v; 46 | Eigen::Matrix dt_hat0, dt_hat, dt; 47 | std::vector t; 48 | Xformd x_e2n_hat; 49 | Vector6d b, bhat; 50 | Matrix2d dt_cov = Vector2d{1e-5, 1e-6}.asDiagonal(); 51 | 52 | std::vector> measurements; 53 | std::vector> cov; 54 | std::vector gtimes; 55 | 56 | ceres::Problem problem; 57 | bool new_node; 58 | std::vector factors; 59 | Imu3DFunctor* factor; 60 | std::vector> new_node_funcs; 61 | 62 | double error0; 63 | 64 | void init(); 65 | void init_residual_blocks(); 66 | void imuCallback(const double& t, const Vector6d& z, const Matrix6d& R) override; 67 | void rawGnssCallback(const GTime& t, const VecVec3& z, const VecMat3& R, std::vector& sats) override; 68 | void init_estimator(); 69 | void addImuFactor(); 70 | void initNodePostionFromPointPos(); 71 | void addPseudorangeFactors(); 72 | void addPseudorangeFactorsWithClockDynamics(); 73 | void addNodeCB(std::function cb); 74 | void run(); 75 | void solve(); 76 | void log(string filename); 77 | double final_error(); 78 | 79 | }; 80 | -------------------------------------------------------------------------------- /include/test_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #define DEG2RAD (M_PI / 180.0) 9 | #define RAD2DEG (180.0 / M_PI) 10 | 11 | #define ASSERT_MAT_EQ(v1, v2) \ 12 | { \ 13 | ASSERT_EQ((v1).rows(), (v2).rows()); \ 14 | ASSERT_EQ((v1).cols(), (v2).cols()); \ 15 | for (int row = 0; row < (v1).rows(); row++) {\ 16 | for (int col = 0; col < (v2).cols(); col++) {\ 17 | ASSERT_FLOAT_EQ((v1)(row, col), (v2)(row,col));\ 18 | }\ 19 | }\ 20 | } 21 | 22 | #define ASSERT_MAT_NEAR(v1, v2, tol) \ 23 | { \ 24 | ASSERT_EQ((v1).rows(), (v2).rows()); \ 25 | ASSERT_EQ((v1).cols(), (v2).cols()); \ 26 | for (int row = 0; row < (v1).rows(); row++) {\ 27 | for (int col = 0; col < (v2).cols(); col++) {\ 28 | ASSERT_NEAR((v1)(row, col), (v2)(row,col), (tol));\ 29 | }\ 30 | }\ 31 | } 32 | 33 | #define EXPECT_MAT_NEAR(v1, v2, tol) \ 34 | { \ 35 | EXPECT_EQ((v1).rows(), (v2).rows()); \ 36 | EXPECT_EQ((v1).cols(), (v2).cols()); \ 37 | for (int row = 0; row < (v1).rows(); row++) {\ 38 | for (int col = 0; col < (v2).cols(); col++) {\ 39 | EXPECT_NEAR((v1)(row, col), (v2)(row,col), (tol));\ 40 | }\ 41 | }\ 42 | } 43 | 44 | #define ASSERT_XFORM_NEAR(x1, x2, tol) \ 45 | { \ 46 | ASSERT_NEAR((x1).t()(0), (x2).t()(0), tol);\ 47 | ASSERT_NEAR((x1).t()(1), (x2).t()(1), tol);\ 48 | ASSERT_NEAR((x1).t()(2), (x2).t()(2), tol);\ 49 | ASSERT_NEAR((x1).q().w(), (x2).q().w(), tol);\ 50 | ASSERT_NEAR((x1).q().x(), (x2).q().x(), tol);\ 51 | ASSERT_NEAR((x1).q().y(), (x2).q().y(), tol);\ 52 | ASSERT_NEAR((x1).q().z(), (x2).q().z(), tol);\ 53 | } 54 | 55 | #define ASSERT_QUAT_NEAR(q1, q2, tol) \ 56 | do { \ 57 | Vector3d qt = (q1) - (q2);\ 58 | ASSERT_LE(std::abs(qt(0)), tol);\ 59 | ASSERT_LE(std::abs(qt(1)), tol);\ 60 | ASSERT_LE(std::abs(qt(2)), tol);\ 61 | } while(0) 62 | 63 | #define EXPECT_QUAT_NEAR(q1, q2, tol) \ 64 | do { \ 65 | Vector3d qt = (q1) - (q2);\ 66 | EXPECT_LE(std::abs(qt(0)), tol);\ 67 | EXPECT_LE(std::abs(qt(1)), tol);\ 68 | EXPECT_LE(std::abs(qt(2)), tol);\ 69 | } while(0) 70 | 71 | std::string imu_only_sim(); 72 | std::string raw_gps_yaml_file(); 73 | std::string raw_gps_multipath_yaml_file(); 74 | std::string mocap_yaml_file(); 75 | 76 | 77 | -------------------------------------------------------------------------------- /lib/factors/include/factors/SE3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "factors/attitude_3d.h" 5 | #include "geometry/xform.h" 6 | 7 | using namespace Eigen; 8 | using namespace xform; 9 | 10 | class XformFunctor 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | XformFunctor(double *x) 15 | { 16 | xform_ = Xformd(x); 17 | } 18 | 19 | template 20 | bool operator()(const T* _x2, T* res) const 21 | { 22 | xform::Xform x2(_x2); 23 | Map> r(res); 24 | r = xform_ - x2; 25 | return true; 26 | } 27 | private: 28 | xform::Xformd xform_; 29 | }; 30 | typedef ceres::AutoDiffCostFunction XformFactorAD; 31 | 32 | 33 | 34 | class XformEdgeFunctor 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | XformEdgeFunctor(Vector7d& _ebar_ij, Matrix6d& _P_ij) 39 | { 40 | ebar_ij_ = _ebar_ij; 41 | Omega_ij_ = _P_ij.inverse(); 42 | } 43 | 44 | template 45 | bool operator()(const T* _xi, const T* _xj, T* _res) const 46 | { 47 | Xform xhat_i(_xi); 48 | Xform xhat_j(_xj); 49 | Xform ehat_12 = xhat_i.inverse() * xhat_j; 50 | Map> res(_res); 51 | res = Omega_ij_ * (ebar_ij_.boxminus(ehat_12)); 52 | return true; 53 | } 54 | private: 55 | xform::Xform ebar_ij_; // Measurement of Edge 56 | Matrix6d Omega_ij_; // Covariance of measurement 57 | }; 58 | typedef ceres::AutoDiffCostFunction XformEdgeFactorAD; 59 | 60 | 61 | 62 | class XformNodeFunctor 63 | { 64 | public: 65 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 66 | XformNodeFunctor(const Vector7d& _xbar, const Matrix6d& _P) 67 | { 68 | xbar_ = Xformd(_xbar); 69 | Omega_ = _P.inverse().llt().matrixL().transpose(); 70 | } 71 | 72 | template 73 | bool operator()(const T* _x, T* _res) const 74 | { 75 | Xform xhat(_x); 76 | Map> res(_res); 77 | res = Omega_ * (xbar_ - xhat); 78 | return true; 79 | } 80 | private: 81 | xform::Xformd xbar_; // Measurement of Node 82 | Matrix6d Omega_; // Covariance of measurement 83 | }; 84 | typedef ceres::AutoDiffCostFunction XformNodeFactorAD; 85 | 86 | 87 | 88 | 89 | struct XformPlus { 90 | template 91 | bool operator()(const T* x, const T* delta, T* x_plus_delta) const 92 | { 93 | Xform q(x); 94 | Map> d(delta); 95 | Map> qp(x_plus_delta); 96 | qp = (q + d).elements(); 97 | return true; 98 | } 99 | }; 100 | typedef ceres::AutoDiffLocalParameterization XformParamAD; 101 | 102 | 103 | struct XformTimeOffsetFunctor 104 | { 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 106 | XformTimeOffsetFunctor(const Vector7d& _x, const Vector6d& _xdot, const Matrix6d& _P) 107 | { 108 | Xi_ = _P.inverse().llt().matrixL().transpose(); 109 | // Xi_ = _P.inverse(); 110 | xdot_ = _xdot; 111 | x_ = _x; 112 | } 113 | 114 | template 115 | bool operator()(const T* _x, const T* _toff, T* _res) const 116 | { 117 | typedef Matrix Vec6; 118 | Map res(_res); 119 | Xform x(_x); 120 | res = Xi_ * ((x_.boxplus((*_toff) * xdot_)) - x); 121 | // res = Xi_ * (x - (x_.boxplus((*_toff) * xdot_))); 122 | return true; 123 | } 124 | private: 125 | Xformd x_; 126 | Vector6d xdot_; 127 | Matrix6d Xi_; 128 | }; 129 | typedef ceres::AutoDiffCostFunction XformTimeOffsetAD; 130 | -------------------------------------------------------------------------------- /lib/factors/include/factors/attitude_3d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "geometry/quat.h" 5 | 6 | using namespace Eigen; 7 | using namespace quat; 8 | 9 | struct QuatPlus { 10 | template 11 | bool operator()(const T* x, const T* delta, T* x_plus_delta) const 12 | { 13 | quat::Quat q(x); 14 | Map> d(delta); 15 | Map> qp(x_plus_delta); 16 | 17 | qp = (q + d).elements(); 18 | return true; 19 | } 20 | }; 21 | typedef ceres::AutoDiffLocalParameterization QuatADParam; 22 | 23 | class QuatParam : public ceres::LocalParameterization 24 | { 25 | public: 26 | ~QuatParam() {} 27 | bool Plus(const double* x, 28 | const double* delta, 29 | double* x_plus_delta) const 30 | { 31 | Quat q(x); 32 | Map d(delta); 33 | Map qp(x_plus_delta); 34 | qp = (q + d).elements(); 35 | return true; 36 | } 37 | 38 | bool ComputeJacobian(const double* x, double* jacobian) const 39 | { 40 | jacobian[0] = -x[1]/2.0; jacobian[1] = -x[2]/2.0; jacobian[2] = -x[3]/2.0; 41 | jacobian[3] = x[0]/2.0; jacobian[4] = -x[3]/2.0; jacobian[5] = x[2]/2.0; 42 | jacobian[6] = x[3]/2.0; jacobian[7] = x[0]/2.0; jacobian[8] = -x[1]/2.0; 43 | jacobian[9] = -x[2]/2.0; jacobian[10] = x[1]/2.0; jacobian[11] = x[0]/2.0; 44 | } 45 | int GlobalSize() const {return 4;} 46 | int LocalSize() const {return 3;} 47 | }; 48 | 49 | class QuatFactor : public ceres::SizedCostFunction<3,4> 50 | { 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | QuatFactor(double *x) 54 | { 55 | quat_.arr_ = Map(x); 56 | } 57 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const 58 | { 59 | Eigen::Map res(residuals); 60 | quat::Quat q2(parameters[0]); 61 | quat::Quat qtilde = q2.inverse().otimes(quat_); 62 | double negative = 1.0; 63 | if (qtilde.w() < 0.0) 64 | { 65 | negative = -1.0; 66 | qtilde.arr_ *= -1.0; 67 | } 68 | res = quat::Quat::log(qtilde); 69 | 70 | 71 | if (jacobians) 72 | { 73 | if (jacobians[0]) 74 | { 75 | double w = qtilde.w(); 76 | Map xyz(qtilde.data() + 1); 77 | double nxyz = xyz.norm(); 78 | Eigen::Map> J(jacobians[0]); 79 | static const Matrix3d I = Eigen::Matrix3d::Identity(); 80 | 81 | double q1w = quat_.arr_(0); 82 | double q1x = quat_.arr_(1); 83 | double q1y = quat_.arr_(2); 84 | double q1z = quat_.arr_(3); 85 | Eigen::Matrix4d Qmat; 86 | Qmat.resize(4,4); 87 | Qmat << q1w, q1x, q1y, q1z, 88 | q1x, -q1w, -q1z, q1y, 89 | q1y, q1z, -q1w, -q1x, 90 | q1z, -q1y, q1x, -q1w; 91 | J.block<3,1>(0,0) = -(2.0 * nxyz) / (nxyz*nxyz + w*w) * xyz / nxyz; 92 | J.block<3,3>(0,1) = (2.0 * w) / (nxyz*nxyz + w*w) * (xyz * xyz.transpose()) / (nxyz*nxyz) 93 | + 2.0 * atan2(nxyz, w) * (I * nxyz*nxyz - xyz * xyz.transpose())/(nxyz*nxyz*nxyz); 94 | J = J * negative * Qmat; 95 | } 96 | } 97 | return true; 98 | } 99 | 100 | private: 101 | quat::Quat quat_; 102 | }; 103 | 104 | class QuatFunctor 105 | { 106 | public: 107 | QuatFunctor(double *x) 108 | { 109 | q_ = Quatd(x); 110 | } 111 | 112 | template 113 | bool operator()(const T* _q2, T* res) const 114 | { 115 | quat::Quat q2(_q2); 116 | Map> r(res); 117 | r = q_.template boxminus(q2); 118 | return true; 119 | } 120 | 121 | private: 122 | quat::Quat q_; 123 | }; 124 | typedef ceres::AutoDiffCostFunction QuatFactorAD; 125 | -------------------------------------------------------------------------------- /lib/factors/include/factors/camera.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "geometry/cam.h" 5 | #include "geometry/xform.h" 6 | 7 | using namespace Eigen; 8 | using namespace xform; 9 | 10 | class CamFunctor 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | CamFunctor(const Vector2d& pix, const Matrix2d& cov, const Vector2d& img_size) 15 | { 16 | pix_ = pix; 17 | size_ = img_size; 18 | Xi_ = cov.inverse().llt().matrixL().transpose(); 19 | } 20 | 21 | template 22 | bool operator()(const T* _ptw, const T* _xw2c, const T* _f, const T* _c, const T* _s, const T* _d, T* _res) const 23 | { 24 | typedef Matrix Vec3; 25 | typedef Matrix Vec2; 26 | 27 | Camera cam(_f, _c, _d, _s); 28 | Map pt_w(_ptw); // point in the world frame 29 | Xform x_w2c(_xw2c); // transform from world to camera 30 | Map res(_res); // residuals 31 | 32 | Vec3 pt_c = x_w2c.transformp(pt_w); // point in the camera frame 33 | Vec2 pixhat; // estimated pixel location 34 | cam.proj(pt_c, pixhat); 35 | 36 | res = Xi_ * (pix_ - pixhat); 37 | return true; 38 | } 39 | 40 | private: 41 | Vector2d pix_; 42 | Vector2d size_; 43 | Matrix2d Xi_; 44 | }; 45 | 46 | typedef ceres::AutoDiffCostFunction CamFactorAD; 47 | -------------------------------------------------------------------------------- /lib/factors/include/factors/carrier_phase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "multirotor_sim/satellite.h" 7 | #include "multirotor_sim/wsg84.h" 8 | 9 | #include "geometry/xform.h" 10 | 11 | 12 | using namespace Eigen; 13 | using namespace xform; 14 | 15 | class CarrierPhaseFunctor 16 | { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | struct SatState 21 | { 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | GTime t; 24 | Vector3d pos; 25 | Vector3d vel; 26 | Vector2d tau; 27 | }; 28 | 29 | 30 | CarrierPhaseFunctor(const GTime& _t0, const GTime& _t1, const double& _dPhi, const Satellite& sat, const Vector3d& _rec_pos_ecef, const double& var) 31 | { 32 | // We don't have ephemeris for this satellite, we can't do anything with it yet 33 | if (sat.eph_.A == 0) 34 | return; 35 | 36 | sats[0].t = _t0; 37 | sats[1].t = _t1; 38 | dPhi = _dPhi; 39 | rec_pos = _rec_pos_ecef; 40 | Xi_ = std::sqrt(1.0/var); 41 | 42 | for (int i = 0; i < 2; i++) 43 | { 44 | sat.computePositionVelocityClock(sats[i].t, sats[i].pos, sats[i].vel, sats[i].tau); 45 | 46 | // Earth rotation correction. The change in velocity can be neglected. 47 | Vector3d los_to_sat = sats[i].pos - rec_pos; 48 | double tof = los_to_sat.norm() / Satellite::C_LIGHT; 49 | sats[i].pos -= sats[i].vel * tof; 50 | double xrot = sats[i].pos.x() + sats[i].pos.y() * Satellite::OMEGA_EARTH * tof; 51 | double yrot = sats[i].pos.y() - sats[i].pos.x() * Satellite::OMEGA_EARTH * tof; 52 | sats[i].pos.x() = xrot; 53 | sats[i].pos.y() = yrot; 54 | } 55 | valid = true; 56 | } 57 | 58 | template 59 | bool operator() (const T* _x0, const T* _x1, const T* _tau0, const T* _tau1, const T* _x_e2n, T* res) const 60 | { 61 | typedef Matrix Vec2; 62 | typedef Matrix Vec3; 63 | 64 | Xform x0(_x0); 65 | Xform x1(_x1); 66 | Xform x_e2n(_x_e2n); 67 | Map tau0(_tau0); 68 | Map tau1(_tau1); 69 | 70 | Vec3 p0_ECEF = x_e2n.transforma(x0.t()); 71 | Vec3 p1_ECEF = x_e2n.transforma(x1.t()); 72 | 73 | T dPhi_hat = (T)(1.0/Satellite::LAMBDA_L1) * (((sats[1].pos - p1_ECEF).norm() + (T)Satellite::C_LIGHT*(tau1[0] - sats[1].tau[0])) 74 | - ((sats[0].pos - p0_ECEF).norm() + (T)Satellite::C_LIGHT*(tau0[0] - sats[0].tau[0]))); 75 | 76 | *res = Xi_ * (dPhi - dPhi_hat); 77 | 78 | return true; 79 | } 80 | 81 | SatState sats[2]; 82 | 83 | bool valid = false; 84 | double dPhi; 85 | Vector3d rec_pos; 86 | double Xi_; 87 | }; 88 | 89 | typedef ceres::AutoDiffCostFunction CarrierPhaseFactorAD; 90 | -------------------------------------------------------------------------------- /lib/factors/include/factors/clock_bias_dynamics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "multirotor_sim/satellite.h" 7 | 8 | using namespace Eigen; 9 | 10 | class ClockBiasFunctor 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | ClockBiasFunctor(const double& dt, const Matrix2d& cov) 15 | { 16 | dt_ = dt; 17 | Xi_ = cov.inverse().llt().matrixL().transpose(); 18 | } 19 | 20 | template 21 | bool operator()(const T* _taui, const T* _tauj, T* _res) const 22 | { 23 | typedef Matrix Vec2; 24 | 25 | Map tau_i(_taui); 26 | Map tau_j(_tauj); 27 | Map res(_res); 28 | 29 | res(0) = (tau_i(0) + tau_i(1) * (T)dt_) - tau_j(0); 30 | res(1) = (tau_i(1)) - tau_j(1); 31 | 32 | res = Xi_ * res; 33 | return true; 34 | } 35 | 36 | double dt_; 37 | Matrix2d Xi_; 38 | }; 39 | typedef ceres::AutoDiffCostFunction ClockBiasFactorAD; 40 | -------------------------------------------------------------------------------- /lib/factors/include/factors/clock_dynamics.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superjax/ceres_sandbox/8207733b59c84e83dcf4f9f4f726ad7d82c3aa48/lib/factors/include/factors/clock_dynamics.h -------------------------------------------------------------------------------- /lib/factors/include/factors/dynamics_1d.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | class Dyn1DFunctor 5 | { 6 | public: 7 | Dyn1DFunctor(double dt) 8 | { 9 | dt_ = dt; 10 | } 11 | 12 | template 13 | bool operator() (const T x0[2], const T x1[2], const T u0[1], const T u1[1], T res[2]) const 14 | { 15 | // trapezoidal integration 16 | res[0] = x1[0] - (x0[0] + (T)0.5 * (T)dt_ * (x1[1] + x0[1])); 17 | res[1] = x1[1] - (x0[1] + (T)0.5 * (T)dt_ * (u1[0] + u0[0])); 18 | 19 | res[0] *= (T) 1e6; 20 | res[1] *= (T) 1e6; 21 | return true; 22 | } 23 | double dt_; 24 | }; 25 | typedef ceres::AutoDiffCostFunction Dyn1dFactorAD; 26 | 27 | 28 | class PosVel1DFunctor 29 | { 30 | public: 31 | PosVel1DFunctor(double x, double v) 32 | { 33 | x_ = x; 34 | v_ = v; 35 | } 36 | template 37 | bool operator() (const T* x, T* res) const 38 | { 39 | res[0] = x[0] - (T)x_; 40 | res[1] = x[1] - (T)v_; 41 | 42 | res[0] *= (T) 1e6; 43 | res[1] *= (T) 1e6; 44 | return true; 45 | } 46 | 47 | double x_; 48 | double v_; 49 | }; 50 | typedef ceres::AutoDiffCostFunction PosVel1DFactorAD; 51 | 52 | 53 | class Input1DFunctor 54 | { 55 | public: 56 | Input1DFunctor(){} 57 | 58 | template 59 | bool operator() (const T* u, T* res) const 60 | { 61 | res[0] = u[0]; 62 | return true; 63 | } 64 | }; 65 | typedef ceres::AutoDiffCostFunction Input1DFactorAD; 66 | -------------------------------------------------------------------------------- /lib/factors/include/factors/dynamics_3d.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geometry/xform.h" 4 | 5 | using namespace Eigen; 6 | using namespace xform; 7 | 8 | struct Dynamics3DPlus { 9 | template 10 | bool operator()(const T* _x, const T* _delta, T* _xp) const 11 | { 12 | Xform x(_x); 13 | Map> d(_delta); 14 | Xform xp(_xp); 15 | xp = x + d; 16 | 17 | Map> v(_x+7); 18 | Map> dv(_delta+6); 19 | Map> vp(_xp+7); 20 | vp = v + dv; 21 | 22 | return true; 23 | } 24 | }; 25 | typedef ceres::AutoDiffLocalParameterization Dynamics3DPlusParamAD; 26 | 27 | 28 | class Dyn3dFunctor 29 | { 30 | public: 31 | Dyn3dFunctor(double dt, double& cost): 32 | cost_{cost} 33 | { 34 | dt_ = dt; 35 | } 36 | 37 | template 38 | bool operator() (const T* _x0, const T* _x1, const T* _u0, const T* _u1, T* res) const 39 | { 40 | 41 | typedef Matrix Vec3; 42 | typedef Matrix Vec9; 43 | Map r(res); 44 | 45 | r.setZero(); 46 | 47 | Map pr(res); 48 | Map qr(res+3); 49 | Map vr(res+6); 50 | 51 | Map p0(_x0); 52 | Map p1(_x1); 53 | Quat q0(_x0+3); 54 | Quat q1(_x1+3); 55 | Map v0(_x0+7); 56 | Map v1(_x0+7); 57 | 58 | Map w0(_u0); 59 | Map w1(_u0); 60 | const T& F0 (*(_u0+3)); 61 | const T& F1 (*(_u1+3)); 62 | 63 | static const Vec3 e_z = (Vec3() << (T)0, (T)0, (T)1).finished(); 64 | static const Vec3 gravity = (Vec3() << (T)0, (T)0, (T)9.80665).finished(); 65 | 66 | Vec3 dv0 = (T)-2.0*9.80665*e_z*F0 - (T)0.9*v0 + q0.rotp(gravity);// - w0.cross(v0); 67 | Vec3 dv1 = (T)-2.0*9.80665*e_z*F1 - (T)0.9*v1 + q1.rotp(gravity);// - w1.cross(v1); 68 | 69 | pr = p1 - (p0 + (T)0.5 * (T)dt_ * (q0.rotp(v0) + q1.rotp(v1))); 70 | qr = q1 - (q0 + (T)0.5 * (T)dt_ * (w0 + w1)); 71 | vr = v1 - (v0 + (T)0.5 * (T)dt_ * (dv0 + dv1)); 72 | // pr = p1 - (p0 + dt_ * q0.rotp(v0)); 73 | // qr = q1 - (q0 + dt_ * w0); 74 | // vr = v1 - (v0 + dt_ * dv0); 75 | 76 | r = cost_ * r; 77 | return true; 78 | } 79 | double dt_; 80 | double& cost_; 81 | }; 82 | typedef ceres::AutoDiffCostFunction Dyn3DFactorAD; 83 | 84 | 85 | class PosVel3DFunctor 86 | { 87 | public: 88 | PosVel3DFunctor(Ref p, double vmag, double& cost) : 89 | cost_(cost) 90 | { 91 | p_ = p; 92 | vmag_ = vmag; 93 | } 94 | template 95 | bool operator() (const T* x, T* res) const 96 | { 97 | typedef Matrix Vec3; 98 | 99 | Map p(x); 100 | Map v(x+7); 101 | 102 | Map pr(res); 103 | T& vr(*(res+3)); 104 | 105 | pr = cost_*(p - p_); 106 | vr = cost_*(v.array().abs().sum() - (T)vmag_); 107 | 108 | return true; 109 | } 110 | 111 | Vector3d p_; 112 | double vmag_; 113 | double& cost_; 114 | }; 115 | typedef ceres::AutoDiffCostFunction PosVel3DFactorAD; 116 | 117 | 118 | class Input3DFunctor 119 | { 120 | public: 121 | Input3DFunctor(Matrix4d& R): 122 | R_(R.data()) 123 | {} 124 | 125 | template 126 | bool operator() (const T* _u, T* res) const 127 | { 128 | typedef Matrix Vec4; 129 | Map u(_u); 130 | Map r(res); 131 | 132 | static const Vec4 eq_input_ = (Vec4() << (T)0, (T)0, (T)0, (T)0.5).finished(); 133 | r = R_*(u - eq_input_); 134 | return true; 135 | } 136 | Map R_; 137 | }; 138 | typedef ceres::AutoDiffCostFunction Input3DFactorAD; 139 | -------------------------------------------------------------------------------- /lib/factors/include/factors/imu_1d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | class Pose1DFactor : public ceres::SizedCostFunction<2,2> 9 | { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | Pose1DFactor(Vector2d z, Matrix2d cov) 13 | { 14 | z_ = z; 15 | Omega_ = cov.inverse(); 16 | } 17 | 18 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const 19 | { 20 | Map x(parameters[0]); 21 | Map r(residuals); 22 | r = x - z_; 23 | r = Omega_ * r; 24 | 25 | if (jacobians) 26 | { 27 | if (jacobians[0]) 28 | { 29 | Map J(jacobians[0]); 30 | J = Omega_; 31 | } 32 | } 33 | return true; 34 | } 35 | 36 | protected: 37 | Vector2d z_; 38 | Matrix2d Omega_; 39 | }; 40 | 41 | class Imu1DFunctor 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | Imu1DFunctor(double _t0, double _bi_hat, double avar) 46 | { 47 | t0_ = _t0; 48 | delta_t_ = 0; 49 | bi_hat_ = _bi_hat; 50 | y_.setZero(); 51 | P_.setZero(); 52 | avar_ = avar; 53 | J_.setZero(); 54 | } 55 | 56 | void integrate(double _t, double a) 57 | { 58 | double dt = _t - (t0_ + delta_t_); 59 | delta_t_ = _t - t0_; 60 | 61 | // propagate covariance 62 | Matrix2d A = (Matrix2d() << 1.0, dt, 0.0, 1.0).finished(); 63 | Vector2d B {0.5*dt*dt, dt}; 64 | Vector2d C {-0.5*dt*dt, dt}; 65 | 66 | // Propagate state 67 | y_ = A*y_ + B*a + C*bi_hat_; 68 | 69 | P_ = A*P_*A.transpose() + B*avar_*B.transpose(); 70 | 71 | // propagate Jacobian dy/db 72 | J_ = A*J_ + C; 73 | } 74 | 75 | Vector2d estimate_xj(const Vector2d& xi) const 76 | { 77 | // Integrate starting at origin pose to get a measurement of the final pose 78 | Vector2d xj; 79 | xj(P) = xi(P) + xi(V)*delta_t_ + y_(ALPHA); 80 | xj(V) = xi(V) + y_(BETA); 81 | return xj; 82 | } 83 | 84 | void finished() 85 | { 86 | Omega_ = P_.inverse(); 87 | } 88 | 89 | template 90 | bool operator()(const T* _xi, const T* _xj, const T* _b, T *residuals) const 91 | { 92 | typedef Matrix Vec2; 93 | Map xi(_xi); 94 | Map xj(_xj); 95 | Map r(residuals); 96 | 97 | // Use the jacobian to re-calculate y_ with change in bias 98 | T db = *_b - bi_hat_; 99 | 100 | Vec2 y_db = y_ + J_ * db; 101 | 102 | r(P) = (xj(P) - xi(P) - xi(V)*delta_t_) - y_db(ALPHA); 103 | r(V) = (xj(V) - xi(V)) - y_db(BETA); 104 | r = Omega_ * r; 105 | 106 | return true; 107 | } 108 | private: 109 | 110 | enum { 111 | ALPHA = 0, 112 | BETA = 1, 113 | }; 114 | enum { 115 | P = 0, 116 | V = 1 117 | }; 118 | 119 | double t0_; 120 | double bi_hat_; 121 | double delta_t_; 122 | Matrix2d P_; 123 | Matrix2d Omega_; 124 | Vector2d y_; 125 | double avar_; 126 | Vector2d J_; 127 | }; 128 | typedef ceres::AutoDiffCostFunction Imu1DFactorAD; 129 | -------------------------------------------------------------------------------- /lib/factors/include/factors/imu_3d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "geometry/xform.h" 7 | 8 | using namespace Eigen; 9 | using namespace xform; 10 | 11 | #pragma once 12 | 13 | #include 14 | #include "geometry/xform.h" 15 | 16 | using namespace Eigen; 17 | using namespace xform; 18 | 19 | #ifndef NDEBUG 20 | #define NANO_IMU_ASSERT(condition, ...) \ 21 | do { \ 22 | if (! (condition)) { \ 23 | std::cerr << "Assertion `" #condition "` failed in " << __FILE__ \ 24 | << " line " << __LINE__ << ": " << printf(__VA_ARGS__) << std::endl; \ 25 | assert(condition); \ 26 | } \ 27 | } while (false) 28 | #else 29 | # define NANO_IMU_ASSERT(...) 30 | #endif 31 | 32 | class Imu3DFunctor 33 | { 34 | public: 35 | enum 36 | { 37 | NRES = 9 38 | }; 39 | typedef Quat QuatT; 40 | typedef Xform XformT; 41 | typedef Matrix Vec3; 42 | typedef Matrix Vec6; 43 | typedef Matrix Vec9; 44 | typedef Matrix Vec10; 45 | 46 | typedef Matrix Mat6; 47 | typedef Matrix Mat9; 48 | typedef Matrix Mat96; 49 | 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | Imu3DFunctor() 52 | { 53 | t0_ = INFINITY; 54 | delta_t_ = 0.0; 55 | b_.setZero(); 56 | y_.setZero(); 57 | P_.setZero(); 58 | J_.setZero(); 59 | } 60 | Imu3DFunctor(const double& _t0, const Vec6& b0) 61 | { 62 | reset(_t0, b0); 63 | } 64 | 65 | 66 | void reset(const double& _t0, const Vec6& b0) 67 | { 68 | delta_t_ = 0.0; 69 | t0_ = _t0; 70 | b_ = b0; 71 | 72 | n_updates_ = 0; 73 | y_.setZero(); 74 | y_(Q) = 1.0; 75 | P_.setZero(); 76 | J_.setZero(); 77 | } 78 | 79 | void errorStateDynamics(const Vec10& y, const Vec9& dy, const Vec6& u, const Vec6& eta, Vec9& dydot) 80 | { 81 | auto dalpha = dy.segment<3>(ALPHA); 82 | auto dbeta = dy.segment<3>(BETA); 83 | QuatT gamma(y.data()+GAMMA); 84 | auto a = u.segment<3>(ACC); 85 | auto w = u.segment<3>(OMEGA); 86 | auto ba = b_.segment<3>(ACC); 87 | auto bw = b_.segment<3>(OMEGA); 88 | auto dgamma = dy.segment<3>(GAMMA); 89 | 90 | auto eta_a = eta.segment<3>(ACC); 91 | auto eta_w = eta.segment<3>(OMEGA); 92 | 93 | dydot.segment<3>(ALPHA) = dbeta; 94 | dydot.segment<3>(BETA) = -gamma.rota(skew(a - ba)*dgamma) - gamma.rota(eta_a); 95 | dydot.segment<3>(GAMMA) = -skew(w - bw)*dgamma - eta_w; 96 | } 97 | 98 | 99 | // ydot = f(y, u) <-- nonlinear dynamics (reference state) 100 | // A = d(dydot)/d(dy) <-- error state 101 | // B = d(dydot)/d(eta) <-- error state 102 | // Because of the error state, ydot != Ay+Bu 103 | void dynamics(const Vec10& y, const Vec6& u, Vec9& ydot, Mat9& A, Mat96&B) 104 | { 105 | auto alpha = y.segment<3>(ALPHA); 106 | auto beta = y.segment<3>(BETA); 107 | QuatT gamma(y.data()+GAMMA); 108 | auto a = u.segment<3>(ACC); 109 | auto w = u.segment<3>(OMEGA); 110 | auto ba = b_.segment<3>(ACC); 111 | auto bw = b_.segment<3>(OMEGA); 112 | 113 | ydot.segment<3>(ALPHA) = beta; 114 | ydot.segment<3>(BETA) = gamma.rota(a - ba); 115 | ydot.segment<3>(GAMMA) = w - bw; 116 | 117 | A.setZero(); 118 | A.block<3,3>(ALPHA, BETA) = I_3x3; 119 | A.block<3,3>(BETA, GAMMA) = -gamma.R().transpose() * skew(a - ba); 120 | A.block<3,3>(GAMMA, GAMMA) = skew(bw-w); 121 | 122 | B.setZero(); 123 | B.block<3,3>(BETA, ACC) = -gamma.R().transpose(); 124 | B.block<3,3>(GAMMA, OMEGA) = -I_3x3; 125 | } 126 | 127 | 128 | static void boxplus(const Vec10& y, const Vec9& dy, Vec10& yp) 129 | { 130 | yp.segment<3>(P) = y.segment<3>(P) + dy.segment<3>(P); 131 | yp.segment<3>(V) = y.segment<3>(V) + dy.segment<3>(V); 132 | yp.segment<4>(Q) = (QuatT(y.segment<4>(Q)) + dy.segment<3>(Q)).elements(); 133 | } 134 | 135 | 136 | static void boxminus(const Vec10& y1, const Vec10& y2, Vec9& d) 137 | { 138 | d.segment<3>(P) = y1.segment<3>(P) - y2.segment<3>(P); 139 | d.segment<3>(V) = y1.segment<3>(V) - y2.segment<3>(V); 140 | d.segment<3>(Q) = QuatT(y1.segment<4>(Q)) - QuatT(y2.segment<4>(Q)); 141 | } 142 | 143 | 144 | void integrate(const double& _t, const Vec6& u, const Mat6& cov) 145 | { 146 | NANO_IMU_ASSERT((cov.array() == cov.array()).all(), "NaN detected in covariance on propagation"); 147 | NANO_IMU_ASSERT((u.array() == u.array()).all(), "NaN detected in covariance on propagation"); 148 | n_updates_++; 149 | double dt = _t - (t0_ + delta_t_); 150 | delta_t_ = _t - t0_; 151 | Vec9 ydot; 152 | Mat9 A; 153 | Mat96 B; 154 | Vec10 yp; 155 | dynamics(y_, u, ydot, A, B); 156 | boxplus(y_, ydot * dt, yp); 157 | y_ = yp; 158 | 159 | A = Mat9::Identity() + A*dt + 1/2.0 * A*A*dt*dt; 160 | B = B*dt; 161 | 162 | P_ = A*P_*A.transpose() + B*cov*B.transpose(); 163 | J_ = A*J_ + B; 164 | 165 | NANO_IMU_ASSERT((A.array() == A.array()).all(), "NaN detected in covariance on propagation"); 166 | NANO_IMU_ASSERT((P_.array() == P_.array()).all(), "NaN detected in covariance on propagation"); 167 | } 168 | 169 | 170 | void estimateXj(const double* _xi, const double* _vi, double* _xj, double* _vj) const 171 | { 172 | auto alpha = y_.segment<3>(ALPHA); 173 | auto beta = y_.segment<3>(BETA); 174 | QuatT gamma(y_.data()+GAMMA); 175 | XformT xi(_xi); 176 | XformT xj(_xj); 177 | Map vi(_vi); 178 | Map vj(_vj); 179 | 180 | xj.t_ = xi.t_ + xi.q_.rota(vi*delta_t_) + 1/2.0 * gravity_*delta_t_*delta_t_ + xi.q_.rota(alpha); 181 | vj = gamma.rotp(vi + xi.q_.rotp(gravity_)*delta_t_ + beta); 182 | xj.q_ = xi.q_ * gamma; 183 | } 184 | 185 | 186 | void finished() 187 | { 188 | if (n_updates_ < 2) 189 | { 190 | P_ = P_ + Mat9::Identity() * 1e-10; 191 | } 192 | Xi_ = P_.inverse().llt().matrixL().transpose(); 193 | NANO_IMU_ASSERT((Xi_.array() == Xi_.array()).all(), "NaN detected in IMU information matrix"); 194 | } 195 | 196 | template 197 | bool operator()(const T* _xi, const T* _xj, const T* _vi, const T* _vj, const T* _b, T* residuals) const 198 | { 199 | typedef Matrix VecT3; 200 | typedef Matrix VecT6; 201 | typedef Matrix VecT9; 202 | typedef Matrix VecT10; 203 | 204 | Xform xi(_xi); 205 | Xform xj(_xj); 206 | Map vi(_vi); 207 | Map vj(_vj); 208 | Map b(_b); 209 | 210 | VecT9 dy = J_ * (b - b_); 211 | VecT10 y; 212 | y.template segment<6>(0) = y_.template segment<6>(0) + dy.template segment<6>(0); 213 | Quat q_dy; 214 | q_dy.arr_(0) = (T)1.0; 215 | q_dy.arr_.template segment<3>(1) = 0.5 * dy.template segment<3>(GAMMA); 216 | y.template segment<4>(6) = (QuatT(y_.template segment<4>(6)).otimes(q_dy)).elements(); 217 | // y.template segment<4>(6) = (QuatT(y_.template segment<4>(6)).otimes(Quat::exp(dy.template segment<3>(6)))).elements(); 218 | 219 | Map alpha(y.data()+ALPHA); 220 | Map beta(y.data()+BETA); 221 | Quat gamma(y.data()+GAMMA); 222 | Map r(residuals); 223 | 224 | r.template block<3,1>(ALPHA, 0) = xi.q_.rotp(xj.t_ - xi.t_ - 1/2.0*gravity_*delta_t_*delta_t_) - vi*delta_t_ - alpha; 225 | r.template block<3,1>(BETA, 0) = gamma.rota(vj) - vi - xi.q_.rotp(gravity_)*delta_t_ - beta; 226 | r.template block<3,1>(GAMMA, 0) = (xi.q_.inverse() * xj.q_) - gamma; 227 | 228 | r = Xi_ * r; 229 | 230 | return true; 231 | } 232 | 233 | enum : int 234 | { 235 | ALPHA = 0, 236 | BETA = 3, 237 | GAMMA = 6, 238 | }; 239 | 240 | enum :int 241 | { 242 | ACC = 0, 243 | OMEGA = 3 244 | }; 245 | 246 | enum : int 247 | { 248 | P = 0, 249 | V = 3, 250 | Q = 6, 251 | }; 252 | 253 | double t0_; 254 | double delta_t_; 255 | Vec6 b_; 256 | int n_updates_ = 0; 257 | 258 | Mat9 P_; 259 | Mat9 Xi_; 260 | Vec10 y_; 261 | 262 | Mat96 J_; 263 | Vec3 gravity_ = (Vec3() << 0, 0, 9.80665).finished(); 264 | }; 265 | 266 | 267 | typedef ceres::AutoDiffCostFunction Imu3DFactorAD; 268 | -------------------------------------------------------------------------------- /lib/factors/include/factors/position_1d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | #include 5 | #include 6 | 7 | 8 | class Pos1DFactor : public ceres::SizedCostFunction<1,1> 9 | { 10 | public: 11 | Pos1DFactor(double x) : 12 | position(x) 13 | {} 14 | 15 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const 16 | { 17 | residuals[0] = position - parameters[0][0]; 18 | 19 | if (jacobians) 20 | { 21 | if (jacobians[0]) 22 | jacobians[0][0] = -1; 23 | } 24 | return true; 25 | } 26 | 27 | protected: 28 | double position; 29 | }; 30 | 31 | class Pos1DTimeOffsetFactor : public ceres::SizedCostFunction<1,2,1> 32 | { 33 | public: 34 | Pos1DTimeOffsetFactor(double x, double cov) : 35 | xbar_(x), cov_(cov) 36 | {} 37 | 38 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const 39 | { 40 | double xhat = parameters[0][0]; 41 | double zhat = parameters[0][1]; 42 | double Tdhat = parameters[1][0]; 43 | 44 | residuals[0] = 1.0/cov_*((xhat + zhat*Tdhat) - xbar_); 45 | 46 | if (jacobians) 47 | { 48 | if (jacobians[0]) 49 | { 50 | jacobians[0][0] = 1.0/cov_; 51 | jacobians[0][1] = Tdhat/cov_; 52 | } 53 | if (jacobians[1]) 54 | { 55 | jacobians[1][0] = zhat/cov_; 56 | } 57 | } 58 | return true; 59 | } 60 | 61 | protected: 62 | double xbar_; 63 | double cov_; 64 | }; 65 | 66 | -------------------------------------------------------------------------------- /lib/factors/include/factors/position_3d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | class Pos3DFactor : public ceres::SizedCostFunction<3,3> 8 | { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | Pos3DFactor(double *x) 12 | { 13 | position_[0] = x[0]; 14 | position_[1] = x[1]; 15 | position_[2] = x[2]; 16 | } 17 | 18 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const 19 | { 20 | Eigen::Map res(residuals); 21 | Eigen::Map z(parameters[0]); 22 | res = position_ - z; 23 | 24 | if (jacobians) 25 | { 26 | if (jacobians[0]) 27 | { 28 | Eigen::Map drdz(jacobians[0]); 29 | drdz = -1.0 * Eigen::Matrix3d::Identity(); 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | protected: 36 | Eigen::Vector3d position_; 37 | }; 38 | -------------------------------------------------------------------------------- /lib/factors/include/factors/pseudorange.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "multirotor_sim/satellite.h" 7 | #include "multirotor_sim/wsg84.h" 8 | 9 | 10 | using namespace Eigen; 11 | 12 | class PRangeFunctor 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | PRangeFunctor(const GTime& _t, const Vector2d& _rho, const Satellite& sat, const Vector3d& _rec_pos_ecef, const Matrix2d& cov) 17 | { 18 | // We don't have ephemeris for this satellite, we can't do anything with it yet 19 | if (sat.eph_.A == 0) 20 | return; 21 | 22 | t = _t; 23 | rho = _rho; 24 | rec_pos = _rec_pos_ecef; 25 | sat.computePositionVelocityClock(t, sat_pos, sat_vel, sat_clk_bias); 26 | 27 | // Earth rotation correction. The change in velocity can be neglected. 28 | Vector3d los_to_sat = sat_pos - rec_pos; 29 | double tau = los_to_sat.norm() / Satellite::C_LIGHT; 30 | sat_pos -= sat_vel * tau; 31 | double xrot = sat_pos.x() + sat_pos.y() * Satellite::OMEGA_EARTH * tau; 32 | double yrot = sat_pos.y() - sat_pos.x() * Satellite::OMEGA_EARTH * tau; 33 | sat_pos.x() = xrot; 34 | sat_pos.y() = yrot; 35 | 36 | los_to_sat = sat_pos - rec_pos; 37 | Vector2d az_el = sat.los2azimuthElevation(rec_pos, los_to_sat); 38 | ion_delay = sat.ionosphericDelay(t, WSG84::ecef2lla(rec_pos), az_el); 39 | Xi_ = cov.inverse().llt().matrixL().transpose(); 40 | valid = true; 41 | } 42 | 43 | template 44 | bool operator()(const T* _x, const T* _v, const T* _clk, const T* _x_e2n, T* _res) const 45 | { 46 | typedef Matrix Vec3; 47 | typedef Matrix Vec2; 48 | 49 | 50 | Xform x(_x); 51 | Map v_b(_v); 52 | Map clk(_clk); 53 | Xform x_e2n(_x_e2n); 54 | Map res(_res); 55 | 56 | 57 | Vec3 v_ECEF = x_e2n.q().rota(x.q().rota(v_b)); 58 | Vec3 p_ECEF = x_e2n.transforma(x.t()); 59 | Vec3 los_to_sat = sat_pos - p_ECEF; 60 | 61 | Vec2 rho_hat; 62 | rho_hat(0) = los_to_sat.norm() + ion_delay + (T)Satellite::C_LIGHT*(clk(0)- sat_clk_bias(0)); 63 | rho_hat(1) = (sat_vel - v_ECEF).dot(los_to_sat.normalized()) + (T)Satellite::C_LIGHT*(clk(1) - sat_clk_bias(1)); 64 | 65 | res = Xi_ * (rho - rho_hat); 66 | 67 | /// TODO: Check if time or rec_pos have deviated too much and re-calculate ion_delay and earth rotation effect 68 | 69 | return true; 70 | } 71 | 72 | bool valid = false; 73 | GTime t; 74 | Vector2d rho; 75 | Vector3d sat_pos; 76 | Vector3d sat_vel; 77 | Vector2d sat_clk_bias; 78 | double ion_delay; 79 | Vector3d rec_pos; 80 | Matrix2d Xi_; 81 | }; 82 | 83 | typedef ceres::AutoDiffCostFunction PRangeFactorAD; 84 | -------------------------------------------------------------------------------- /lib/factors/include/factors/range_1d.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class Range1DFactor : public ceres::SizedCostFunction<1,1,1> 4 | { 5 | public: 6 | Range1DFactor(double z, double var) : 7 | range_(z), 8 | var_(var) 9 | {} 10 | 11 | virtual bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 12 | { 13 | double l = parameters[0][0]; 14 | double x = parameters[1][0]; 15 | residuals[0] = (range_ - (l - x)) / var_; 16 | 17 | if (jacobians) 18 | { 19 | if (jacobians[0]) 20 | { 21 | jacobians[0][0] = -1/var_; 22 | } 23 | if (jacobians[1]) 24 | { 25 | jacobians[1][0] = 1/var_; 26 | } 27 | } 28 | return true; 29 | } 30 | 31 | private: 32 | double range_; 33 | double var_; 34 | 35 | }; 36 | 37 | class RangeVel1DFactor : public ceres::SizedCostFunction<1,1,2> 38 | { 39 | public: 40 | RangeVel1DFactor(double z, double var) : 41 | range_(z), 42 | var_(var) 43 | {} 44 | 45 | virtual bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 46 | { 47 | double l = parameters[0][0]; 48 | double x = parameters[1][0]; 49 | residuals[0] = (range_ - (l - x)) / var_; 50 | if (jacobians) 51 | { 52 | if (jacobians[0]) 53 | { 54 | jacobians[0][0] = -1.0/var_; 55 | } 56 | if (jacobians[1]) 57 | { 58 | jacobians[1][0] = 1.0/var_; 59 | jacobians[1][1] = 0; 60 | } 61 | } 62 | return true; 63 | } 64 | 65 | private: 66 | double range_; 67 | double var_; 68 | 69 | }; 70 | -------------------------------------------------------------------------------- /lib/factors/include/factors/switch_pseudorange.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "factors/pseudorange.h" 5 | 6 | class SwitchPRangeFunctor : public PRangeFunctor 7 | { 8 | public: 9 | SwitchPRangeFunctor(const GTime& _t, const Vector2d& _rho, const Satellite& sat, 10 | const Vector3d& _rec_pos_ecef, const Matrix2d& cov, const double& s0, 11 | const double& s_weight) 12 | : PRangeFunctor(_t, _rho, sat, _rec_pos_ecef, cov) 13 | { 14 | s0_ = s0; 15 | sw_ = s_weight; 16 | } 17 | 18 | void reset(double s0) 19 | { 20 | s0_ = s0; 21 | } 22 | 23 | template 24 | bool operator()(const T* _x, const T* _v, const T* _clk, const T* _x_e2n, const T* _s, T* _res) const 25 | { 26 | bool result = PRangeFunctor::operator ()(_x, _v, _clk, _x_e2n, _res); 27 | T s = *_s; 28 | if (s < 0.0) 29 | s = (T)0.0; 30 | else if (s > 1.0) 31 | s = (T)1.0; 32 | 33 | _res[0] *= s; 34 | _res[1] *= s; 35 | _res[2] = sw_ * (s0_ - s); 36 | return result; 37 | } 38 | 39 | double s0_; 40 | double sw_; 41 | }; 42 | 43 | typedef ceres::AutoDiffCostFunction SwitchPRangeFactorAD; 44 | 45 | 46 | class SwitchDynamicsFunctor 47 | { 48 | public: 49 | SwitchDynamicsFunctor(const double& weight) 50 | { 51 | sw_ = weight; 52 | } 53 | 54 | template 55 | bool operator()(const T* _si, const T* _sj, T* _res) const 56 | { 57 | *_res = sw_ * (*_si - *_sj); 58 | return true; 59 | } 60 | 61 | double sw_; 62 | }; 63 | typedef ceres::AutoDiffCostFunction SwitchDynamicsFactorAD; 64 | -------------------------------------------------------------------------------- /lib/factors/include/factors/transform_1d.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class Transform1DFactor : public ceres::SizedCostFunction<1,1,1> 4 | { 5 | public: 6 | Transform1DFactor(double z, double var) : 7 | transform_(z), 8 | var_(var) 9 | {} 10 | 11 | virtual bool Evaluate(const double * const *parameters, double *residuals, double **jacobians) const 12 | { 13 | double xi = parameters[0][0]; 14 | double xj = parameters[1][0]; 15 | residuals[0] = (transform_ - (xj - xi))/var_; 16 | 17 | if (jacobians) 18 | { 19 | if (jacobians[0]) 20 | { 21 | jacobians[0][0] = 1.0/var_; 22 | } 23 | if (jacobians[1]) 24 | { 25 | jacobians[1][0] = -1.0/var_; 26 | } 27 | } 28 | return true; 29 | } 30 | 31 | private: 32 | double transform_; 33 | double var_; 34 | }; 35 | -------------------------------------------------------------------------------- /lib/factors/src/imu_3d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "geometry/xform.h" 5 | 6 | #include "factors/imu_3d.h" 7 | 8 | using namespace Eigen; 9 | using namespace xform; 10 | -------------------------------------------------------------------------------- /lib/utils/include/utils/jac.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Eigen::MatrixXd calc_jac(std::function fun, const Eigen::MatrixXd &x, 4 | std::functionx_boxminus=nullptr, 5 | std::functionx_boxplus=nullptr, 6 | std::function y_boxminus=nullptr, double step_size=1e-8); 7 | -------------------------------------------------------------------------------- /lib/utils/include/utils/logger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "multirotor_sim/utils.h" 6 | 7 | 8 | template 9 | class Logger 10 | { 11 | public: 12 | Logger(std::string filename) 13 | { 14 | createDirIfNotExist(std::experimental::filesystem::path(filename).parent_path()); 15 | file_.open(filename); 16 | } 17 | 18 | ~Logger() 19 | { 20 | file_.close(); 21 | } 22 | template 23 | void log(T... data) 24 | { 25 | int dummy[sizeof...(data)] = { (file_.write((char*)&data, sizeof(Scalar)), 1)... }; 26 | } 27 | 28 | template 29 | void logVectors(T... data) 30 | { 31 | int dummy[sizeof...(data)] = { (file_.write((char*)data.data(), sizeof(Scalar)*data.rows()*data.cols()), 1)... }; 32 | } 33 | 34 | private: 35 | std::ofstream file_; 36 | }; 37 | -------------------------------------------------------------------------------- /lib/utils/include/utils/robot1d.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | using namespace Eigen; 9 | 10 | class Robot1D 11 | { 12 | public: 13 | Robot1D(double _ba, double Q, double Td=0.0); 14 | 15 | void add_waypoint(double wp); 16 | 17 | void step(double dt); 18 | 19 | double xhat_; 20 | double vhat_; 21 | double ahat_; 22 | double t_; 23 | std::vector waypoints_; 24 | 25 | typedef struct 26 | { 27 | double x; 28 | double v; 29 | double t; 30 | double xhat; 31 | double vhat; 32 | } history_t; 33 | std::deque hist_; 34 | 35 | double Td_; 36 | double b_; 37 | double x_; 38 | double v_; 39 | double a_; 40 | double kp_; 41 | double kd_; 42 | double prev_x_; 43 | int i_; 44 | 45 | double a_stdev_; 46 | double b_stdev_; 47 | 48 | std::default_random_engine gen_; 49 | std::normal_distribution normal_; 50 | }; 51 | -------------------------------------------------------------------------------- /lib/utils/include/utils/trajectory_sim.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "geometry/xform.h" 10 | #include "multirotor_sim/simulator.h" 11 | #include "multirotor_sim/controller.h" 12 | #include "multirotor_sim/satellite.h" 13 | #include "multirotor_sim/estimator_wrapper.h" 14 | #include "utils/logger.h" 15 | #include "test_common.h" 16 | 17 | #include "factors/SE3.h" 18 | #include "factors/imu_3d.h" 19 | #include "factors/switch_pseudorange.h" 20 | #include "factors/pseudorange.h" 21 | #include "factors/carrier_phase.h" 22 | #include "factors/clock_bias_dynamics.h" 23 | 24 | 25 | using namespace multirotor_sim; 26 | using namespace Eigen; 27 | using namespace xform; 28 | 29 | class TrajectorySim : public EstimatorBase 30 | { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | int n = 0; 34 | Simulator sim; 35 | static const int N = 100; 36 | Eigen::Matrix xhat0, xhat, x; 37 | Eigen::Matrix vhat0, vhat, v; 38 | Eigen::Matrix dt_hat0, dt_hat, dt; 39 | Eigen::Matrix shat0, shat, s; 40 | std::vector t; 41 | Xformd x_e2n_hat; 42 | Vector6d b, bhat; 43 | Matrix2d dt_cov = Vector2d{1e-5, 1e-6}.asDiagonal(); 44 | 45 | std::vector> measurements; 46 | std::vector> cov; 47 | std::vector gtimes; 48 | 49 | ceres::Problem problem; 50 | bool new_node; 51 | std::vector factors; 52 | Imu3DFunctor* factor; 53 | std::vector> new_node_funcs; 54 | 55 | GTime t0; 56 | std::vector Phi0; 57 | 58 | double switch_weight; 59 | double switch_dyn_weight; 60 | 61 | double error0; 62 | 63 | TrajectorySim(const string &yaml_file); 64 | void init(const std::string& yaml_file); 65 | void init_residual_blocks(); 66 | void imuCallback(const double& t, const Vector6d& z, const Matrix6d& R) override; 67 | void rawGnssCallback(const GTime& t, const VecVec3& z, const VecMat3& R, std::vector& sats, const std::vector& slip) override; 68 | void init_estimator(); 69 | void fix_origin(); 70 | void addImuFactor(); 71 | void initNodePostionFromPointPos(); 72 | void addPseudorangeFactors(); 73 | void addPseudorangeFactorsWithClockDynamics(); 74 | void addSwitchingPseudorangeFactors(); 75 | void addCarrierPhaseFactors(); 76 | void addNodeCB(std::function cb); 77 | void run(); 78 | void solve(); 79 | void log(string filename); 80 | double final_error(); 81 | 82 | 83 | }; 84 | 85 | -------------------------------------------------------------------------------- /lib/utils/src/jac.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "utils/jac.h" 3 | 4 | Eigen::MatrixXd calc_jac(std::function fun, const Eigen::MatrixXd& x, 5 | std::functionx_boxminus, 6 | std::functionx_boxplus, 7 | std::functiony_boxminus, 8 | double step_size) 9 | { 10 | if (x_boxminus == nullptr) 11 | { 12 | x_boxminus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& x2) 13 | { 14 | return x1 - x2; 15 | }; 16 | } 17 | if (y_boxminus == nullptr) 18 | { 19 | y_boxminus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& x2) 20 | { 21 | return x1 - x2; 22 | }; 23 | } 24 | 25 | if (x_boxplus == nullptr) 26 | { 27 | x_boxplus = [](const Eigen::MatrixXd& x1, const Eigen::MatrixXd& dx) 28 | { 29 | return x1 + dx; 30 | }; 31 | } 32 | 33 | Eigen::MatrixXd y = fun(x); 34 | Eigen::MatrixXd dy = y_boxminus(y,y); 35 | int rows = dy.rows(); 36 | 37 | Eigen::MatrixXd dx = x_boxminus(x, x); 38 | int cols = dx.rows(); 39 | 40 | Eigen::MatrixXd I; 41 | I.resize(cols, cols); 42 | I.setZero(cols, cols); 43 | for (int i = 0; i < cols; i++) 44 | I(i,i) = step_size; 45 | 46 | Eigen::MatrixXd JFD; 47 | JFD.setZero(rows, cols); 48 | for (int i =0; i < cols; i++) 49 | { 50 | Eigen::MatrixXd xp = x_boxplus(x, I.col(i)); 51 | Eigen::MatrixXd xm = x_boxplus(x, -1.0*I.col(i)); 52 | 53 | Eigen::MatrixXd yp = fun(xp); 54 | Eigen::MatrixXd ym = fun(xm); 55 | Eigen::MatrixXd dy = y_boxminus(yp,ym); 56 | 57 | JFD.col(i) = dy/(2*step_size); 58 | } 59 | return JFD; 60 | } 61 | -------------------------------------------------------------------------------- /lib/utils/src/robot1d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "utils/robot1d.h" 9 | 10 | using namespace Eigen; 11 | 12 | Robot1D::Robot1D(double _ba, double Q, double Td) : 13 | normal_(0.0, 1.0) 14 | { 15 | x_ = 0; 16 | a_ = 0; 17 | v_ = 0; 18 | i_ = 0; 19 | t_ = 0; 20 | prev_x_ = NAN; 21 | kp_ = 0.3; 22 | kd_ = 0.003; 23 | b_ = _ba; 24 | Td_ = Td; 25 | 26 | a_stdev_ = Q; 27 | b_stdev_ = 0.0; 28 | 29 | xhat_ = x_; 30 | vhat_ = v_; 31 | ahat_ = a_; 32 | 33 | // Create a history 34 | history_t history {x_, v_, t_, xhat_, vhat_}; 35 | hist_.push_back(history); 36 | } 37 | 38 | void Robot1D::add_waypoint(double wp) 39 | { 40 | waypoints_.push_back(wp); 41 | } 42 | 43 | void Robot1D::step(double dt) 44 | { 45 | if (std::abs(x_ - waypoints_[i_]) < 1e-2) 46 | i_ = (i_ + 1) % waypoints_.size(); 47 | 48 | // propagate dynamics 49 | double e = waypoints_[i_] - x_; 50 | if (!std::isfinite(prev_x_)) 51 | prev_x_ = x_; 52 | a_ = kp_*e + kd_ * (prev_x_ - x_)/dt; 53 | x_ += v_ * dt; 54 | v_ += a_ * dt; 55 | t_ += dt; 56 | b_ += normal_(gen_)*b_stdev_*dt; 57 | 58 | // propagate estimates 59 | ahat_ = a_ + normal_(gen_)*a_stdev_ - b_; 60 | 61 | // Save history 62 | history_t history {x_, v_, t_, xhat_, vhat_}; 63 | hist_.push_back(history); 64 | while (t_ - hist_.front().t > Td_ && hist_.size() > 0) 65 | { 66 | hist_.pop_front(); 67 | } 68 | } 69 | 70 | -------------------------------------------------------------------------------- /lib/utils/src/trajectory_sim.cpp: -------------------------------------------------------------------------------- 1 | #include "utils/trajectory_sim.h" 2 | 3 | TrajectorySim::TrajectorySim(const string& yaml_file) : 4 | sim(false, 2) 5 | { 6 | init(yaml_file); 7 | init_residual_blocks(); 8 | init_estimator(); 9 | } 10 | 11 | void TrajectorySim::init(const std::string& yaml_file) 12 | { 13 | sim.load(yaml_file); 14 | 15 | switch_weight = 10.0; 16 | switch_dyn_weight = 2.0; 17 | 18 | x_e2n_hat = sim.X_e2n_; 19 | b << sim.accel_bias_, sim.gyro_bias_; 20 | bhat.setZero(); 21 | 22 | measurements.resize(sim.satellites_.size()); 23 | gtimes.resize(sim.satellites_.size()); 24 | cov.resize(sim.satellites_.size()); 25 | n = 0; 26 | new_node = false; 27 | s.resize(sim.satellites_.size(), N); 28 | shat.resizeLike(s); 29 | } 30 | 31 | void TrajectorySim::fix_origin() 32 | { 33 | problem.AddResidualBlock(new XformNodeFactorAD(new XformNodeFunctor(sim.state().X.arr(), Matrix6d::Identity()*1e-6)), 34 | NULL, xhat.data()); 35 | } 36 | 37 | void TrajectorySim::init_residual_blocks() 38 | { 39 | vhat.setZero(); 40 | dt_hat.setZero(); 41 | shat.setConstant(1.0); 42 | for (int i = 0; i < N; i++) 43 | { 44 | xhat.col(i) = Xformd::Identity().elements(); 45 | problem.AddParameterBlock(xhat.data() + i*7, 7, new XformParamAD()); 46 | problem.AddParameterBlock(vhat.data() + i*3, 3); 47 | problem.AddParameterBlock(dt_hat.data()+i*2, 2); 48 | } 49 | problem.AddParameterBlock(x_e2n_hat.data(), 7, new XformParamAD()); 50 | problem.SetParameterBlockConstant(x_e2n_hat.data()); 51 | 52 | } 53 | 54 | 55 | 56 | void TrajectorySim::imuCallback(const double& t, const Vector6d& z, const Matrix6d& R) 57 | { 58 | factor->integrate(t, z, R); 59 | } 60 | 61 | void TrajectorySim::rawGnssCallback(const GTime& t, const VecVec3& z, const VecMat3& R, std::vector& sats, const std::vector& slip) 62 | { 63 | int i = 0; 64 | for (auto sat : sats) 65 | { 66 | measurements[sat.idx_] = z[i]; 67 | cov[sat.idx_] = R[i]; 68 | gtimes[sat.idx_] = t; 69 | i++; 70 | new_node = true; 71 | } 72 | } 73 | 74 | void TrajectorySim::init_estimator() 75 | { 76 | xhat.col(0) = sim.state().X.arr(); 77 | x.col(0) = sim.state().X.arr(); 78 | factors.push_back(new Imu3DFunctor(0, bhat)); 79 | factor = factors[0]; 80 | sim.register_estimator(this); 81 | t.push_back(sim.t_); 82 | } 83 | 84 | void TrajectorySim::addImuFactor() 85 | { 86 | if (n < 1) 87 | return; 88 | factor->estimateXj(xhat.data()+7*(n-1), vhat.data()+3*(n-1), xhat.data()+7*(n), vhat.data()+3*(n)); 89 | factor->finished(); 90 | problem.AddResidualBlock(new Imu3DFactorAD(factor), NULL, 91 | xhat.data()+7*(n-1), xhat.data()+7*(n), 92 | vhat.data()+3*(n-1), vhat.data()+3*(n), 93 | bhat.data()); 94 | factors.push_back(new Imu3DFunctor(sim.t_, bhat)); 95 | factor = factors.back(); 96 | } 97 | 98 | void TrajectorySim::initNodePostionFromPointPos() 99 | { 100 | Vector3d xn_ecef = sim.X_e2n_.t(); 101 | WSG84::pointPositioning(gtimes[0], measurements, sim.satellites_, xn_ecef); 102 | xhat.template block<3,1>(0, n) = WSG84::ecef2ned(sim.X_e2n_, xn_ecef); 103 | } 104 | 105 | void TrajectorySim::addPseudorangeFactors() 106 | { 107 | for (int i = 0; i < measurements.size(); i++) 108 | { 109 | problem.AddResidualBlock(new PRangeFactorAD(new PRangeFunctor(gtimes[i], 110 | measurements[i].topRows<2>(), 111 | sim.satellites_[i], 112 | sim.get_position_ecef(), 113 | cov[i].topLeftCorner<2,2>())), 114 | NULL, 115 | xhat.data() + (n)*7, 116 | vhat.data() + (n)*3, 117 | dt_hat.data(), 118 | x_e2n_hat.data()); 119 | s(i, n) = (double)(sim.multipath_offset_[i] == 0); 120 | } 121 | } 122 | 123 | void TrajectorySim::addPseudorangeFactorsWithClockDynamics() 124 | { 125 | for (int i = 0; i < measurements.size(); i++) 126 | { 127 | problem.AddResidualBlock(new PRangeFactorAD(new PRangeFunctor(gtimes[i], 128 | measurements[i].topRows<2>(), 129 | sim.satellites_[i], 130 | sim.get_position_ecef(), 131 | cov[i].topLeftCorner<2,2>())), 132 | NULL, 133 | xhat.data() + (n)*7, 134 | vhat.data() + (n)*3, 135 | dt_hat.data() + (n)*2, 136 | x_e2n_hat.data()); 137 | s(i, n) = (double)(sim.multipath_offset_[i] == 0); 138 | } 139 | if (n < 1) 140 | return; 141 | 142 | double deltat = sim.t_ - t.back(); 143 | dt_hat(0, n) = dt_hat(0, n-1) + deltat * dt_hat(1,n-1); 144 | dt_hat(1, n) = dt_hat(1, n-1); 145 | 146 | problem.AddResidualBlock(new ClockBiasFactorAD(new ClockBiasFunctor(deltat, dt_cov)), 147 | NULL, dt_hat.data() + (n-1)*2, dt_hat.data() + n*2); 148 | 149 | } 150 | 151 | void TrajectorySim::addSwitchingPseudorangeFactors() 152 | { 153 | for (int i = 0; i < measurements.size(); i++) 154 | { 155 | problem.AddResidualBlock(new SwitchPRangeFactorAD(new SwitchPRangeFunctor(gtimes[i], 156 | measurements[i].topRows<2>(), 157 | sim.satellites_[i], 158 | sim.get_position_ecef(), 159 | cov[i].topLeftCorner<2,2>(), 160 | shat(i,n), switch_weight)), 161 | NULL, 162 | xhat.data() + (n)*7, 163 | vhat.data() + (n)*3, 164 | dt_hat.data() + (n)*2, 165 | x_e2n_hat.data(), 166 | shat.data() + s.rows()*n+i); 167 | s(i, n) = (double)(sim.multipath_offset_[i] == 0); 168 | if (n > 1) 169 | problem.AddResidualBlock(new SwitchDynamicsFactorAD(new SwitchDynamicsFunctor(switch_dyn_weight)), 170 | NULL, shat.data() + (n-1)*s.rows() + i, shat.data() + n*s.rows() + i); 171 | } 172 | if (n < 1) 173 | return; 174 | 175 | double deltat = sim.t_ - t.back(); 176 | dt_hat(0, n) = dt_hat(0, n-1) + deltat * dt_hat(1,n-1); 177 | dt_hat(1, n) = dt_hat(1, n-1); 178 | 179 | problem.AddResidualBlock(new ClockBiasFactorAD(new ClockBiasFunctor(deltat, dt_cov)), 180 | NULL, dt_hat.data() + (n-1)*2, dt_hat.data() + n*2); 181 | } 182 | 183 | void TrajectorySim::addCarrierPhaseFactors() 184 | { 185 | if (n < 1) 186 | { 187 | t0 = gtimes[0]; 188 | Phi0.resize(measurements.size()); 189 | for (int i = 0; i < measurements.size(); i++) 190 | { 191 | Phi0[i] = measurements[i](2); 192 | } 193 | return; 194 | } 195 | 196 | for (int i = 0; i < measurements.size(); i++) 197 | { 198 | problem.AddResidualBlock(new CarrierPhaseFactorAD( 199 | new CarrierPhaseFunctor(t0, 200 | gtimes[i], 201 | measurements[i](2) - Phi0[i], 202 | sim.satellites_[i], 203 | sim.get_position_ecef(), 204 | cov[i](2,2))), 205 | NULL, 206 | xhat.data(), 207 | xhat.data()+n*7, 208 | dt_hat.data(), 209 | dt_hat.data() + n*2, 210 | x_e2n_hat.data()); 211 | } 212 | } 213 | 214 | void TrajectorySim::addNodeCB(std::function cb) 215 | { 216 | new_node_funcs.push_back(cb); 217 | } 218 | 219 | void TrajectorySim::run() 220 | { 221 | // problem.AddResidualBlock(new XformNodeFactorAD(new XformNodeFunctor(sim.state().X.arr(), Matrix6d::Identity() * 1e-8)), 222 | // NULL, xhat.data()); 223 | while (n < N) 224 | { 225 | sim.run(); 226 | 227 | if (new_node) 228 | { 229 | new_node = false; 230 | for (auto fun : new_node_funcs) 231 | { 232 | fun(); 233 | } 234 | 235 | x.col(n) = sim.state().X.elements(); 236 | v.col(n) = sim.state().v; 237 | dt.col(n) << sim.clock_bias_, sim.clock_bias_rate_ ; 238 | n++; 239 | t.push_back(sim.t_); 240 | } 241 | } 242 | } 243 | 244 | void TrajectorySim::solve() 245 | { 246 | xhat0 = xhat; 247 | vhat0 = vhat; 248 | dt_hat0 = dt_hat; 249 | shat0 = shat; 250 | error0 = (xhat - x).array().abs().sum(); 251 | 252 | 253 | ceres::Solver::Options options; 254 | options.max_num_iterations = 100; 255 | options.num_threads = 6; 256 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 257 | options.minimizer_progress_to_stdout = false; 258 | ceres::Solver::Summary summary; 259 | 260 | ceres::Solve(options, &problem, &summary); 261 | } 262 | 263 | void TrajectorySim::log(string filename) 264 | { 265 | Logger log(filename); 266 | 267 | for (int i = 0; i < N; i++) 268 | { 269 | log.log(t[i]); 270 | log.logVectors(xhat0.col(i), 271 | vhat0.col(i), 272 | xhat.col(i), 273 | vhat.col(i), 274 | x.col(i), 275 | v.col(i), 276 | dt_hat0.col(i), 277 | dt_hat.col(i), 278 | dt.col(i), 279 | s.col(i), 280 | shat.col(i), 281 | shat0.col(i)); 282 | } 283 | } 284 | 285 | double TrajectorySim::TrajectorySim::final_error() 286 | { 287 | return (xhat - x).array().abs().sum(); 288 | } 289 | -------------------------------------------------------------------------------- /matlab/Imu3DCheckPropagation.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | file = fopen('/tmp/ceres_sandbox/Imu3D.CheckPropagation.log', 'r'); 5 | 6 | data = fread(file, 'double'); 7 | data = reshape(data, 1+7+3+7+3+6 + 4, [])'; 8 | 9 | t = data(:, 1); 10 | xhat = data(:, 2:11); 11 | x = data(:, 12:21); 12 | u = data(:, 22:27); 13 | test = data(:, 28:end); 14 | 15 | names = ["px", "py", "pz", "qw", "qx", "qy", "qz", "vx", "vy", "vz"]; 16 | 17 | 18 | figure(1); clf; 19 | set(gcf, 'name', 'Position', 'NumberTitle', 'off'); 20 | for i =1:3 21 | idx = i; 22 | subplot(3, 1, i) 23 | plot(t, x(:,idx)) 24 | hold on; 25 | plot(t, xhat(:,idx)) 26 | title(names(idx)); 27 | legend("x", "xhat") 28 | end 29 | 30 | 31 | figure(2); clf; 32 | set(gcf, 'name', 'Attitude', 'NumberTitle', 'off'); 33 | for i =1:4 34 | idx = i+3; 35 | subplot(4, 1, i) 36 | plot(t, x(:,idx)) 37 | hold on; 38 | plot(t, xhat(:,idx)) 39 | plot(t, test(:,i)) 40 | title(names(idx)); 41 | legend("x", "xhat") 42 | end 43 | 44 | 45 | figure(3); clf; 46 | set(gcf, 'name', 'Velocity', 'NumberTitle', 'off'); 47 | for i =1:3 48 | idx = i+7; 49 | subplot(3, 1, i) 50 | plot(t, x(:,idx)) 51 | hold on; 52 | plot(t, xhat(:,idx)) 53 | title(names(idx)); 54 | legend("x", "xhat") 55 | end 56 | 57 | names = ["ax", "ay", "az", "wx", "wy", "wz"]; 58 | figure(4); clf; 59 | set(gcf, 'name', 'Input', 'NumberTitle', 'off'); 60 | for j = 1:2 61 | for i =1:3 62 | idx = i + (j-1)*3; 63 | subplot(3, 2, (i-1)*2+j) 64 | plot(t, u(:,idx)) 65 | hold on; 66 | title(names(idx)); 67 | end 68 | end 69 | -------------------------------------------------------------------------------- /matlab/Imu3DMultiWindowPlot.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | truth_file = fopen('/tmp/ceres_sandbox/Imu3d.MultiWindow.truth.log', 'r'); 5 | est_file = fopen('/tmp/ceres_sandbox/Imu3d.MultiWindow.est.log', 'r'); 6 | est0_file = fopen('/tmp/ceres_sandbox/Imu3d.MultiWindow.est0.log', 'r'); 7 | 8 | truth = fread(truth_file, 'double'); 9 | truth = reshape(truth, 11, []); 10 | 11 | est = fread(est_file, 'double'); 12 | est = reshape(est, 11, []); 13 | 14 | est0 = fread(est0_file, 'double'); 15 | est0 = reshape(est0, 11, []); 16 | 17 | names = ["t", "px", "py", "pz", "qw", "qx", "qy", "qz", "vx", "vy", "vz"]; 18 | 19 | 20 | %% Plot position 21 | figure(1); clf; 22 | set(gcf, 'name', 'Position', 'NumberTitle', 'off'); 23 | for i = 1:3 24 | idx = i+1; 25 | subplot(3,1,i); 26 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 27 | hold on; 28 | plot(est0(1,:), est0(idx,:)); 29 | plot(est(1,:), est(idx,:)); 30 | title(names(idx)); 31 | legend("truth", "est0", "estf") 32 | end 33 | 34 | %% Plot attitude 35 | figure(2); clf; 36 | set(gcf, 'name', 'Attitude', 'NumberTitle', 'off'); 37 | for i = 1:4 38 | idx = i+4; 39 | subplot(4,1,i); 40 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 41 | hold on; 42 | plot(est0(1,:), est0(idx,:)); 43 | plot(est(1,:), est(idx,:)); 44 | title(names(idx)); 45 | legend("truth", "est0", "estf") 46 | end 47 | 48 | 49 | %% Plot velocity 50 | figure(3); clf; 51 | set(gcf, 'name', 'Velocity', 'NumberTitle', 'off'); 52 | for i = 1:3 53 | idx = i+8; 54 | subplot(3,1,i); 55 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 56 | hold on; 57 | plot(est0(1,:), est0(idx,:)); 58 | plot(est(1,:), est(idx,:)); 59 | title(names(idx)); 60 | legend("truth", "est0", "est") 61 | end -------------------------------------------------------------------------------- /matlab/TimeOffsetMultiWindowPlot.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | truth_file = fopen('/tmp/ceres_sandbox/TimeOffset.MultiWindowConstantBias.truth.log', 'r'); 5 | est_file = fopen('/tmp/ceres_sandbox/TimeOffset.MultiWindowConstantBias.est.log', 'r'); 6 | est0_file = fopen('/tmp/ceres_sandbox/TimeOffset.MultiWindowConstantBias.est0.log', 'r'); 7 | 8 | truth = fread(truth_file, 'double'); 9 | truth = reshape(truth, 11, []); 10 | 11 | est = fread(est_file, 'double'); 12 | est = reshape(est, 11, []); 13 | 14 | est0 = fread(est0_file, 'double'); 15 | est0 = reshape(est0, 11, []); 16 | 17 | names = ["t", "px", "py", "pz", "qw", "qx", "qy", "qz", "vx", "vy", "vz"]; 18 | 19 | 20 | %% Plot position 21 | figure(1); clf; 22 | set(gcf, 'name', 'Position', 'NumberTitle', 'off'); 23 | for i = 1:3 24 | idx = i+1; 25 | subplot(3,1,i); 26 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 27 | hold on; 28 | plot(est0(1,:), est0(idx,:)); 29 | plot(est(1,:), est(idx,:)); 30 | title(names(idx)); 31 | legend("truth", "est0", "estf") 32 | ylim([-15, 15]); 33 | end 34 | 35 | %% Plot attitude 36 | figure(2); clf; 37 | set(gcf, 'name', 'Attitude', 'NumberTitle', 'off'); 38 | for i = 1:4 39 | idx = i+4; 40 | subplot(4,1,i); 41 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 42 | hold on; 43 | plot(est0(1,:), est0(idx,:)); 44 | plot(est(1,:), est(idx,:)); 45 | title(names(idx)); 46 | legend("truth", "est0", "estf") 47 | end 48 | 49 | 50 | %% Plot velocity 51 | figure(3); clf; 52 | set(gcf, 'name', 'Velocity', 'NumberTitle', 'off'); 53 | for i = 1:3 54 | idx = i+8; 55 | subplot(3,1,i); 56 | plot(truth(1,:), truth(idx,:), 'lineWidth', 1.2); 57 | hold on; 58 | plot(est0(1,:), est0(idx,:)); 59 | plot(est(1,:), est(idx,:)); 60 | title(names(idx)); 61 | legend("truth", "est0", "est") 62 | end -------------------------------------------------------------------------------- /matlab/TrajectoryOptimization1DMultiPlot.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | state_file = fopen('/tmp/ceres_sandbox/1d_trajectory_multi_states.bin', 'r'); 5 | init_file = fopen('/tmp/ceres_sandbox/1d_trajectory_multi_states_init.bin', 'r'); 6 | input_file = fopen('/tmp/ceres_sandbox/1d_trajectory_multi_input.bin', 'r'); 7 | 8 | x = reshape(fread(state_file, 'double'), 2, []); 9 | x0 = reshape(fread(init_file, 'double'), 2, []); 10 | input = fread(input_file, 'double'); 11 | time = linspace(0, 1, length(x)); 12 | 13 | %% Plot Trajectory 14 | figure(2); clf; 15 | set(gcf, 'name', 'Trajectory1D', 'NumberTitle', 'off'); 16 | subplot(3,1,1); hold on; 17 | ylabel("x (m)") 18 | plot(time, x(1,:)) 19 | plot(time, x0(1,:)) 20 | legend("x", "x0"); 21 | subplot(3,1,2); hold on; 22 | ylabel("v (m/s)") 23 | plot(time, x(2,:)) 24 | plot(time, x0(2,:)) 25 | subplot(3,1,3); hold on; 26 | ylabel("F (N)"); 27 | plot(time, input) 28 | -------------------------------------------------------------------------------- /matlab/TrajectoryOptimization1DSinglePlot.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | state_file = fopen('/tmp/ceres_sandbox/1d_trajectory_states.bin', 'r'); 5 | init_file = fopen('/tmp/ceres_sandbox/1d_trajectory_states_init.bin', 'r'); 6 | input_file = fopen('/tmp/ceres_sandbox/1d_trajectory_input.bin', 'r'); 7 | 8 | x = reshape(fread(state_file, 'double'), 2, []); 9 | x0 = reshape(fread(init_file, 'double'), 2, []); 10 | input = fread(input_file, 'double'); 11 | time = linspace(0, 1, length(x)); 12 | 13 | %% Plot Trajectory 14 | figure(1); clf; 15 | set(gcf, 'name', 'Trajectory1D', 'NumberTitle', 'off'); 16 | subplot(3,1,1); hold on; 17 | ylabel("x (m)") 18 | plot(time, x(1,:)) 19 | plot(time, x0(1,:)) 20 | legend("x", "x0"); 21 | subplot(3,1,2); hold on; 22 | ylabel("v (m/s)") 23 | plot(time, x(2,:)) 24 | plot(time, x0(2,:)) 25 | subplot(3,1,3); hold on; 26 | ylabel("F (N)"); 27 | plot(time, input) 28 | -------------------------------------------------------------------------------- /matlab/TrajectoryOptimization3DSinglePlot.m: -------------------------------------------------------------------------------- 1 | format compact 2 | set(0,'DefaultFigureWindowStyle','docked') 3 | 4 | state_file = fopen('/tmp/ceres_sandbox/3d_trajectory_states_init.bin', 'r'); 5 | init_file = fopen('/tmp/ceres_sandbox/3d_trajectory_states.bin', 'r'); 6 | input_file = fopen('/tmp/ceres_sandbox/3d_trajectory_input.bin', 'r'); 7 | 8 | x = reshape(fread(state_file, 'double'), 10, []); 9 | x0 = reshape(fread(init_file, 'double'), 10, []); 10 | input = reshape(fread(input_file, 'double'), 4, []); 11 | [rows, cols] = size(x0); 12 | time = linspace(0, 1, cols); 13 | 14 | labels =["px", "py", "pz", "qw", "qx", "qy", "qz", "vx", "vy", "vz", "wx", "wy", "wz", "F"]; 15 | 16 | %% Plot Position 17 | figure(1); clf; 18 | set(gcf, 'name', 'Trajectory3DPosition', 'NumberTitle', 'off'); 19 | for i = 1:3 20 | subplot(3,1,i); hold on; 21 | ylabel(labels(i)) 22 | plot(time, x(i,:)) 23 | plot(time, x0(i,:)) 24 | end 25 | 26 | %% Plot Attitude 27 | figure(2); clf; 28 | set(gcf, 'name', 'Trajectory3DAttitude', 'NumberTitle', 'off'); 29 | for i = 1:4 30 | idx = i + 3; 31 | subplot(4,1,i); hold on; 32 | ylabel(labels(idx)) 33 | plot(time, x(idx,:)) 34 | plot(time, x0(idx,:)) 35 | end 36 | 37 | 38 | %% Plot Velocity 39 | figure(3); clf; 40 | set(gcf, 'name', 'Trajectory3DVelocity', 'NumberTitle', 'off'); 41 | for i = 1:3 42 | idx = i + 7; 43 | subplot(3,1,i); hold on; 44 | ylabel(labels(idx)) 45 | plot(time, x(idx,:)) 46 | plot(time, x0(idx,:)) 47 | end 48 | 49 | %% Plot Inputs 50 | figure(4); clf; 51 | set(gcf, 'name', 'Trajectory3DInputs', 'NumberTitle', 'off'); 52 | for i = 1:4 53 | idx = i + 10; 54 | subplot(4,1,i); hold on; 55 | ylabel(labels(idx)) 56 | plot(time, input(i,:)) 57 | end -------------------------------------------------------------------------------- /python/CarrierPhase.ImuTrajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/CarrierPhase.ImuTrajectory.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "CPhase, ImuTraj") -------------------------------------------------------------------------------- /python/CarrierPhase.Trajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/CarrierPhase.Trajectory.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "CPhase, Traj") -------------------------------------------------------------------------------- /python/Imu3D_MultiWindow.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from plotWindow import plotWindow 4 | 5 | np.set_printoptions(linewidth=150) 6 | plt.rc('text', usetex=True) 7 | plt.rc('font', family='serif') 8 | pw = plotWindow() 9 | 10 | LOG_WIDTH = 1 + 7 + 3 + 7 + 3 + 7 + 3 11 | 12 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/Imu3d.MultiWindow.log', dtype=np.float64), (-1, LOG_WIDTH)) 13 | 14 | t = data[:,0] 15 | xhat0 = data[:,1:8] 16 | vhat0 = data[:,8:11] 17 | xhatf = data[:, 11:18] 18 | vhatf = data[:, 18:21] 19 | x = data[:, 21:28] 20 | v = data[:, 28:31] 21 | 22 | xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] 23 | vtitles = ['vx', 'vy', 'vz'] 24 | 25 | f = plt.figure(1) 26 | for i in range(3): 27 | plt.subplot(3, 1, i+1) 28 | plt.title(xtitles[i]) 29 | plt.plot(t, xhat0[:,i], label="$\hat{x}_0$") 30 | plt.plot(t, xhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 31 | plt.plot(t, x[:,i], label="$x$") 32 | plt.legend() 33 | pw.addPlot("Position", f) 34 | 35 | f = plt.figure(2) 36 | for i in range(4): 37 | plt.subplot(4, 1, i+1) 38 | plt.title(xtitles[i+3]) 39 | plt.plot(t, xhat0[:,i+3], label="$\hat{x}_0$") 40 | plt.plot(t, xhatf[:,i+3], '--', linewidth=3, label="$\hat{x}_f$") 41 | plt.plot(t, x[:,i+3], label="$x$") 42 | plt.legend() 43 | pw.addPlot("Attitude", f) 44 | 45 | f = plt.figure(3) 46 | for i in range(3): 47 | plt.subplot(3, 1, i+1) 48 | plt.title(vtitles[i]) 49 | plt.plot(t, vhat0[:,i], label="$\hat{x}_0$") 50 | plt.plot(t, vhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 51 | plt.plot(t, v[:,i], label="$x$") 52 | plt.legend() 53 | pw.addPlot("Velocity", f) 54 | 55 | pw.show() 56 | 57 | -------------------------------------------------------------------------------- /python/Imu3D_Propagation.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | from plotWindow import plotWindow 5 | 6 | np.set_printoptions(linewidth=150) 7 | plt.rc('text', usetex=True) 8 | plt.rc('font', family='serif') 9 | pw = plotWindow() 10 | 11 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/Imu3D.CheckPropagation.log', dtype=np.float64), (-1,27)) 12 | 13 | t = data[:, 0] 14 | xhat = data[:, 1:11] 15 | x = data[:, 11:21] 16 | u = data[:, 21:27] 17 | 18 | error = x - xhat 19 | 20 | 21 | 22 | f = plt.figure() 23 | plt.suptitle('Position') 24 | for i in range(3): 25 | plt.subplot(3, 1, i+1) 26 | plt.plot(t, x[:,i], label='x') 27 | plt.plot(t, xhat[:,i], label=r'\hat{x}') 28 | if i == 0: 29 | plt.legend() 30 | pw.addPlot("Position", f) 31 | 32 | f = plt.figure() 33 | plt.suptitle('Velocity') 34 | for i in range(3): 35 | plt.subplot(3, 1, i+1) 36 | plt.plot(t, x[:,i+7], label='x') 37 | plt.plot(t, xhat[:, i+7], label=r'$\hat{x}$') 38 | if i == 0: 39 | plt.legend() 40 | pw.addPlot("Velocity", f) 41 | 42 | f = plt.figure() 43 | plt.suptitle('Attitude') 44 | for i in range(4): 45 | plt.subplot(4, 1, i+1) 46 | plt.plot(t, x[:,i+3], label='x') 47 | plt.plot(t, xhat[:,i+3], label=r'\hat{x}') 48 | if i == 0: 49 | plt.legend() 50 | pw.addPlot("Attitude", f) 51 | 52 | f = plt.figure() 53 | plt.suptitle('Input') 54 | labels=['a_x', 'a_y', 'a_z', r'$\omega_x$', r'$\omega_{y}$', r'$\omega_{z}$'] 55 | for j in range(2): 56 | for i in range(3): 57 | plt.subplot(3, 2, i*2+1 + j) 58 | plt.plot(t, u[:,i+j*3], label=labels[i+j*3]) 59 | plt.legend() 60 | pw.addPlot("Input", f) 61 | 62 | f = plt.figure() 63 | plt.suptitle('Error') 64 | labels=[r'$p_x$', r'$p_y$', r'$p_z$', 65 | r'$v_x$', r'$v_y$', r'$v_z$', 66 | r'$q_x$', r'$q_y$', r'$q_z$'] 67 | for j in range(3): 68 | for i in range(3): 69 | plt.subplot(3, 3, i*3+1+j) 70 | plt.plot(t, error[:,i+j*3], label=labels[i+j*3]) 71 | plt.legend() 72 | 73 | pw.addPlot("Error", f) 74 | 75 | pw.show() -------------------------------------------------------------------------------- /python/MultipathPseudorange.StandardResidual.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/MultipathPseudorange.StandardResidual.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "Multipath, Standard") 7 | 8 | -------------------------------------------------------------------------------- /python/MultipathPseudorange.SwitchedResidual.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/MultipathPseudorange.SwitchingResidual.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "Multipath, Switched") 7 | 8 | -------------------------------------------------------------------------------- /python/Pseudorange.ImuTrajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/Pseudorange.ImuTrajectory.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "Prange, ImuTraj") -------------------------------------------------------------------------------- /python/Pseudorange.Trajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/Pseudorange.Trajectory.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "Prange, Traj") -------------------------------------------------------------------------------- /python/Pseudorange.TrajectoryClockDynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from plotRawGPS import plotRawGPS, LOG_WIDTH 3 | 4 | data = np.reshape(np.fromfile('/tmp/ceres_sandbox/Pseudorange.ImuTrajectoryClockDynamics.log', dtype=np.float64), (-1, LOG_WIDTH)) 5 | 6 | plotRawGPS(data, "PRange, ImuTrajClk") 7 | 8 | -------------------------------------------------------------------------------- /python/TimeOffset.3DmultirotorPoseGraph.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from plotWindow import plotWindow 4 | 5 | np.set_printoptions(linewidth=150) 6 | plt.rc('text', usetex=True) 7 | plt.rc('font', family='serif') 8 | pw = plotWindow() 9 | 10 | LOG_WIDTH = 1 + 7 + 3 + 7 + 3 + 7 + 3 11 | 12 | data = np.reshape(np.fromfile("/tmp/ceres_sandbox/TimeOffset.MultiWindowConstantBias.log", dtype=np.float64), (-1, LOG_WIDTH)) 13 | 14 | t = data[:,0] 15 | xhat0 = data[:,1:8] 16 | vhat0 = data[:,8:11] 17 | xhatf = data[:, 11:18] 18 | vhatf = data[:, 18:21] 19 | x = data[:, 21:28] 20 | v = data[:, 28:31] 21 | 22 | xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] 23 | vtitles = ['vx', 'vy', 'vz'] 24 | 25 | f = plt.figure(1) 26 | for i in range(3): 27 | plt.subplot(3, 1, i+1) 28 | plt.title(xtitles[i]) 29 | plt.plot(t, xhat0[:,i], label="$\hat{x}_0$") 30 | plt.plot(t, xhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 31 | plt.plot(t, x[:,i], label="$x$") 32 | plt.legend() 33 | pw.addPlot("Position", f) 34 | 35 | f = plt.figure(2) 36 | for i in range(4): 37 | plt.subplot(4, 1, i+1) 38 | plt.title(xtitles[i+3]) 39 | plt.plot(t, xhat0[:,i+3], label="$\hat{x}_0$") 40 | plt.plot(t, xhatf[:,i+3], '--', linewidth=3, label="$\hat{x}_f$") 41 | plt.plot(t, x[:,i+3], label="$x$") 42 | plt.legend() 43 | pw.addPlot("Attitude", f) 44 | 45 | f = plt.figure(3) 46 | for i in range(3): 47 | plt.subplot(3, 1, i+1) 48 | plt.title(vtitles[i]) 49 | plt.plot(t, vhat0[:,i], label="$\hat{x}_0$") 50 | plt.plot(t, vhatf[:,i], '--', linewidth=3, label="$\hat{x}_f$") 51 | plt.plot(t, v[:,i], label="$x$") 52 | plt.legend() 53 | pw.addPlot("Velocity", f) 54 | 55 | pw.show() 56 | 57 | -------------------------------------------------------------------------------- /python/plotRawGPS.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from plotWindow import plotWindow 4 | 5 | 6 | LOG_WIDTH = 1 + 7 + 3 + 7 + 3 + 7 + 3 + 2 + 2 + 2 + 15 + 15 + 15 7 | def plotRawGPS(data, title="raw_gps"): 8 | np.set_printoptions(linewidth=150) 9 | plt.rc('text', usetex=True) 10 | plt.rc('font', family='serif') 11 | 12 | t = data[:,0] 13 | xhat0 = data[:,1:8] 14 | vhat0 = data[:,8:11] 15 | xhatf = data[:, 11:18] 16 | vhatf = data[:, 18:21] 17 | x = data[:, 21:28] 18 | v = data[:, 28:31] 19 | dthat0 = data[:, 31:33] 20 | dthatf = data[:, 33:35] 21 | dt = data[:,35:37] 22 | s = data[:, 37:52] 23 | shatf = data[:, 52:67] 24 | shat0 = data[:, 67:82] 25 | 26 | xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz'] 27 | vtitles = ['vx', 'vy', 'vz'] 28 | dttitles = [r'$\tau$', r'$\dot{\tau}$'] 29 | 30 | 31 | 32 | pw = plotWindow(title=title) 33 | 34 | f = plt.figure(dpi=150) 35 | plt.plot(-xhat0[:,1], xhat0[:,0], label=r"$\hat{x}_0$") 36 | plt.plot(-xhatf[:,1], xhatf[:,0], '--', linewidth=3, label=r"$\hat{x}_f$") 37 | plt.plot(-x[:,1], x[:,0], label="$x$") 38 | plt.legend() 39 | pw.addPlot("Position2D", f) 40 | 41 | f = plt.figure(dpi=150) 42 | for i in range(3): 43 | plt.subplot(3, 1, i+1) 44 | plt.title(xtitles[i]) 45 | plt.plot(t, xhat0[:,i], label=r"$\hat{x}_0$") 46 | plt.plot(t, xhatf[:,i], '--', linewidth=3, label=r"$\hat{x}_f$") 47 | plt.plot(t, x[:,i], label="$x$") 48 | plt.legend() 49 | pw.addPlot("Position", f) 50 | 51 | 52 | f = plt.figure(dpi=150) 53 | for i in range(4): 54 | plt.subplot(4, 1, i+1) 55 | plt.title(xtitles[i+3]) 56 | plt.plot(t, xhat0[:,i+3], label=r"$\hat{x}_0$") 57 | plt.plot(t, xhatf[:,i+3], '--', linewidth=3, label=r"$\hat{x}_f$") 58 | plt.plot(t, x[:,i+3], label="$x$") 59 | plt.legend() 60 | pw.addPlot("Attitude", f) 61 | 62 | f = plt.figure(dpi=150) 63 | for i in range(3): 64 | plt.subplot(3, 1, i+1) 65 | plt.title(vtitles[i]) 66 | plt.plot(t, vhat0[:,i], label=r"$\hat{x}_0$") 67 | plt.plot(t, vhatf[:,i], '--', linewidth=3, label=r"$\hat{x}_f$") 68 | plt.plot(t, v[:,i], label="$x$") 69 | plt.legend() 70 | pw.addPlot("Velocity", f) 71 | 72 | f = plt.figure() 73 | for i in range(2): 74 | plt.subplot(2, 1, i+1) 75 | plt.title(dttitles[i]) 76 | plt.plot(t, dthat0[:,i], label=r"$\hat{x}_0$") 77 | plt.plot(t, dthatf[:,i], '--', linewidth=3, label=r"$\hat{x}_f$") 78 | plt.plot(t, dt[:,i], label="$x$") 79 | plt.legend() 80 | pw.addPlot("Clock Bias", f) 81 | 82 | f = plt.figure() 83 | plt.suptitle("Switch Factor") 84 | for i in range(15): 85 | plt.subplot(15, 1, i+1) 86 | plt.plot(t, shat0[:,i], "-r", label=r"$\hat{x}_0$") 87 | plt.plot(t, shatf[:,i], "-b", label=r"$\hat{x}_f$") 88 | plt.plot(t, s[:,i], "-g", label=r"x") 89 | plt.ylim(-0.1, 1.1) 90 | if i == 0: 91 | plt.legend(ncol=3) 92 | pw.addPlot("Switch Factor", f) 93 | 94 | 95 | pw.show() -------------------------------------------------------------------------------- /python/plotWindow.py: -------------------------------------------------------------------------------- 1 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 2 | from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from PyQt5.QtWidgets import QMainWindow, QApplication, QPushButton, QWidget, QAction, QTabWidget,QVBoxLayout 6 | from PyQt5.QtGui import QIcon 7 | from PyQt5.QtCore import pyqtSlot 8 | import sys 9 | 10 | 11 | class plotWindow(): 12 | def __init__(self, title="plot_window", parent=None): 13 | self.app = QApplication(sys.argv) 14 | self.MainWindow = QMainWindow() 15 | self.MainWindow.__init__() 16 | self.MainWindow.setWindowTitle(title) 17 | self.canvases = [] 18 | self.figure_handles = [] 19 | self.toolbar_handles = [] 20 | self.tab_handles = [] 21 | self.current_window = -1 22 | self.tabs = QTabWidget() 23 | self.MainWindow.setCentralWidget(self.tabs) 24 | self.MainWindow.resize(1280, 900) 25 | self.MainWindow.show() 26 | 27 | def addPlot(self, title, figure): 28 | new_tab = QWidget() 29 | layout = QVBoxLayout() 30 | new_tab.setLayout(layout) 31 | 32 | figure.subplots_adjust(left=0.05, right=0.99, bottom=0.05, top=0.91, wspace=0.2, hspace=0.2) 33 | new_canvas = FigureCanvas(figure) 34 | new_toolbar = NavigationToolbar(new_canvas, new_tab) 35 | 36 | layout.addWidget(new_canvas) 37 | layout.addWidget(new_toolbar) 38 | self.tabs.addTab(new_tab, title) 39 | 40 | self.toolbar_handles.append(new_toolbar) 41 | self.canvases.append(new_canvas) 42 | self.figure_handles.append(figure) 43 | self.tab_handles.append(new_tab) 44 | 45 | def show(self): 46 | self.app.exec_() 47 | 48 | if __name__ == '__main__': 49 | import numpy as np 50 | 51 | 52 | pw = plotWindow() 53 | 54 | x = np.arange(0, 10, 0.001) 55 | 56 | f = plt.figure() 57 | ysin = np.sin(x) 58 | plt.plot(x, ysin, '--') 59 | pw.addPlot("sin", f) 60 | 61 | f = plt.figure() 62 | ycos = np.cos(x) 63 | plt.plot(x, ycos, '--') 64 | pw.addPlot("cos", f) 65 | pw.show() 66 | 67 | # sys.exit(app.exec_()) 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /python/plotWindow.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superjax/ceres_sandbox/8207733b59c84e83dcf4f9f4f726ad7d82c3aa48/python/plotWindow.pyc -------------------------------------------------------------------------------- /scripts/build_and_run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd .. 4 | mkdir -p build 5 | cd build 6 | cmake .. -DCMAKE_BUILD_TYPE=Release 7 | make -j12 -l12 8 | ./test_attitude 9 | ./test_camera 10 | ./test_carrier_phas 11 | ./test_control 12 | ./test_imu1d 13 | ./test_imu3d 14 | ./test_pose 15 | ./test_position1d 16 | ./test_position3d 17 | ./test_pseudorange 18 | ./test_switch 19 | ./test_time_offset 20 | -------------------------------------------------------------------------------- /scripts/jac_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def norm(v, axis=None): 4 | return np.sqrt(np.sum(v*v, axis=axis)) 5 | 6 | def calc_jac(fun, x, args=None): 7 | 8 | y = fun(x) if args is None else fun(x, *args) 9 | if (isinstance(x, float)): cols = 1 10 | else: cols = x.shape[0] 11 | if (isinstance(y, float)): rows = 1 12 | else: rows = y.shape[0] 13 | 14 | I = np.eye(cols)*1e-6 15 | JFD = np.zeros((rows, cols)) 16 | for i in range(cols): 17 | xp = x + I[:,i,None] 18 | xm = x - I[:, i, None] 19 | yp = fun(xp) if args is None else fun(xp, *args) 20 | ym = fun(xm) if args is None else fun(xm, *args) 21 | JFD[:,i,None] = (yp - ym)/(2*1e-6) 22 | return JFD 23 | 24 | def cos_norm_delta_over_2(delta): 25 | return np.cos(norm(delta/2.0)) 26 | 27 | def exp_vector(delta): 28 | return np.sin(norm(delta/2.0)) * delta / norm(delta) 29 | 30 | def logp1(w, xyz): 31 | return 2 * np.arctan2(norm(xyz), w) * xyz / norm(xyz) 32 | 33 | def logp2(xyz, w): 34 | return 2 * np.arctan2(norm(xyz), w) * xyz / norm(xyz) 35 | 36 | def invotimes(q2, q1): 37 | q1w = q1[0, 0] 38 | q1x = q1[1, 0] 39 | q1y = q1[2, 0] 40 | q1z = q1[3, 0] 41 | q2w = q2[0, 0] 42 | q2x = q2[1, 0] 43 | q2y = q2[2, 0] 44 | q2z = q2[3, 0] 45 | 46 | return np.array([[q2w*q1w + q2x*q1x + q2y*q1y + q2z*q1z], 47 | [q2w*q1x - q2x*q1w - q2y*q1z + q2z*q1y], 48 | [q2w*q1y + q2x*q1z - q2y*q1w - q2z*q1x], 49 | [q2w*q1z - q2x*q1y + q2y*q1x - q2z*q1w]]) 50 | 51 | def boxminus(q2, q1): 52 | qtilde = invotimes(q2, q1) 53 | w = qtilde[0,0] 54 | xyz = qtilde[1:,:] 55 | nxyz = norm(xyz) 56 | return 2.0 * np.arctan2(nxyz, w) * xyz / nxyz 57 | 58 | if __name__ == '__main__': 59 | 60 | print(r"d/ddelta cos (\norm{\delta / 2}) = ") 61 | x = np.random.randn(3,1) 62 | JA = -np.sin(norm(x/2.0)) * 0.5/norm(x) * x.T 63 | JFD = calc_jac(cos_norm_delta_over_2, x) 64 | print(JA) 65 | print(JFD) 66 | 67 | print(r"d/ddelta sin (\norm{\delta / 2}) * \norm{\delta} / (2 * delta) = ") 68 | x = np.random.randn(3,1) 69 | JA = np.cos(norm(x/2.0)) * 0.5/norm(x) * x * x.T/norm(x) + np.sin(norm(x/2.0)) * (np.eye(3) * norm(x)**2 - x * x.T)/norm(x)**3 70 | JFD = calc_jac(exp_vector, x) 71 | print(JA) 72 | print(JFD) 73 | 74 | quat = np.random.randn(4,1) 75 | quat /= norm(quat) 76 | w = quat[0,0] 77 | xyz = quat[1:,:] 78 | print("log p1: d/dqw atan2") 79 | nxyz = norm(xyz) 80 | JA = - (2 * nxyz) / (nxyz**2 + w**2) * xyz / nxyz 81 | JFD = calc_jac(logp1, w, [xyz]) 82 | print(JA) 83 | print(JFD) 84 | print(JA - JFD) 85 | 86 | print("log p2: d/dqxyz atan2") 87 | JA2 = (2 * w) / (nxyz**2 + w**2) * (xyz * xyz.T) / nxyz \ 88 | + 2 * np.arctan2(nxyz, w) * ((np.eye(3) * nxyz**2 - xyz.dot(xyz.T))/nxyz**3) 89 | JFD = calc_jac(logp2, xyz, [w]) 90 | print(JA2) 91 | print(JFD) 92 | print(JA2 - JFD) 93 | 94 | print("d/dq2 invotimes") 95 | q2 = np.random.randn(4, 1) 96 | q2 /= norm(q2) 97 | q1w = quat[0, 0] 98 | q1x = -quat[1, 0] 99 | q1y = -quat[2, 0] 100 | q1z = -quat[3, 0] 101 | Qmat = np.array([[q1w, -q1x, -q1y, -q1z], 102 | [-q1x, -q1w, q1z, -q1y], 103 | [-q1y, -q1z, -q1w, q1x], 104 | [-q1z, q1y, -q1x, -q1w]]) 105 | JFD = calc_jac(invotimes, q2, [quat]) 106 | print(JA) 107 | print(JFD) 108 | print(Qmat - JFD) 109 | 110 | print("big cahuna d/dq2 boxminus") 111 | qtilde = invotimes(q2, quat) 112 | w = qtilde[0,0] 113 | xyz = qtilde[1:,:] 114 | 115 | B1 =- (2 * nxyz) / (nxyz**2 + w**2) * xyz / nxyz 116 | B2 = (2 * w) / (nxyz**2 + w**2) * (xyz .dot(xyz.T)) / nxyz \ 117 | + 2 * np.arctan2(nxyz, w) * ((np.eye(3) * nxyz**2 - xyz.dot(xyz.T))/nxyz**3) 118 | JA = np.hstack((B1, B2)).dot(Qmat) 119 | JFD = calc_jac(boxminus, q2, [quat]) 120 | print(JA) 121 | print(JFD) 122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /src/attitude.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "factors/attitude_3d.h" 6 | #include "utils/jac.h" 7 | #include "geometry/quat.h" 8 | 9 | 10 | using namespace ceres; 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | TEST(Attitude3d, CheckLocalParamPlus) 15 | { 16 | 17 | for (int i = 0; i < 1000; i++) 18 | { 19 | QuatParam* param = new QuatParam(); 20 | 21 | quat::Quat x = quat::Quat::Random(); 22 | Eigen::Vector3d delta; 23 | delta.setRandom(); 24 | quat::Quat xplus1, xplus2; 25 | param->Plus(x.data(), delta.data(), xplus1.data()); 26 | xplus2 = x + delta; 27 | EXPECT_FLOAT_EQ(xplus1.w(), xplus2.w()); 28 | EXPECT_FLOAT_EQ(xplus1.x(), xplus2.x()); 29 | EXPECT_FLOAT_EQ(xplus1.y(), xplus2.y()); 30 | EXPECT_FLOAT_EQ(xplus1.z(), xplus2.z()); 31 | } 32 | } 33 | 34 | TEST(Attitude3d, CheckFactorEvaluate) 35 | { 36 | 37 | for (int i = 0; i < 1000; i++) 38 | { 39 | quat::Quat x1 = quat::Quat::Random(); 40 | quat::Quat x2 = quat::Quat::Random(); 41 | Vector3d delta1, delta2; 42 | QuatFactor* factor = new QuatFactor(x1.data()); 43 | double* p[1]; 44 | p[0] = x2.data(); 45 | factor->Evaluate(p, delta1.data(), 0); 46 | delta2 = x1 - x2; 47 | EXPECT_FLOAT_EQ(delta1(0), delta2(0)); 48 | EXPECT_FLOAT_EQ(delta1(1), delta2(1)); 49 | EXPECT_FLOAT_EQ(delta1(2), delta2(2)); 50 | } 51 | } 52 | 53 | TEST(Attitude3d, CheckFactorJac) 54 | { 55 | quat::Quat x1 = quat::Quat::Random(); 56 | quat::Quat x2 = quat::Quat::Random(); 57 | Vector3d delta1, delta2; 58 | QuatFactor* factor = new QuatFactor(x1.data()); 59 | double* p[1]; 60 | p[0] = x2.data(); 61 | 62 | Eigen::Matrix J, JFD; 63 | double* jdata[1]; 64 | jdata[0] = J.data(); 65 | 66 | 67 | factor->Evaluate(p, delta1.data(), jdata); 68 | delta2 = x1 - x2; 69 | 70 | Eigen::Matrix I4x4 = Eigen::Matrix::Identity(); 71 | I4x4 *= 1e-8; 72 | Vector4d x2primeplus, x2primeminus; 73 | Vector3d delta1primeplus, delta1primeminus; 74 | for (int i = 0; i < 4; i++) 75 | { 76 | x2primeplus = x2.arr_ + I4x4.col(i); 77 | double * pprimeplus[1]; 78 | pprimeplus[0] = x2primeplus.data(); 79 | factor->Evaluate(pprimeplus, delta1primeplus.data(), 0); 80 | 81 | x2primeminus = x2.arr_ - I4x4.col(i); 82 | double * pprimeminus[1]; 83 | pprimeminus[0] = x2primeminus.data(); 84 | factor->Evaluate(pprimeminus, delta1primeminus.data(), 0); 85 | 86 | 87 | JFD.col(i) = (delta1primeplus - delta1primeminus) / 2e-8; 88 | } 89 | 90 | double err = (J - JFD).array().abs().sum(); 91 | 92 | if (err > 1e-6) 93 | { 94 | std::cout << "J\n" << J << "\n"; 95 | std::cout << "JFD\n" << JFD << "\n"; 96 | } 97 | EXPECT_LE(err, 1e-6); 98 | } 99 | 100 | TEST(Attitude3d, CheckLocalParamJac) 101 | { 102 | QuatParam* param = new QuatParam(); 103 | 104 | quat::Quat x = quat::Quat::Random(); 105 | Eigen::Vector3d delta; 106 | delta.setZero(); // Jacobian is evaluated at dx = 0 107 | quat::Quat xplus1, xplus2; 108 | param->Plus(x.data(), delta.data(), xplus1.data()); 109 | xplus2 = x + delta; 110 | 111 | Eigen::Matrix J, JFD; 112 | param->ComputeJacobian(x.data(), J.data()); 113 | 114 | Matrix3d I3x3 = Matrix3d::Identity() * 1e-8; 115 | Vector3d deltaprimeplus; 116 | Vector3d deltaprimeminus; 117 | Vector4d xplusprimeplus; 118 | Vector4d xplusprimeminus; 119 | for (int i = 0; i < 3; i++) 120 | { 121 | deltaprimeplus = delta + I3x3.col(i); 122 | deltaprimeminus = delta - I3x3.col(i); 123 | param->Plus(x.data(), deltaprimeplus.data(), xplusprimeplus.data()); 124 | param->Plus(x.data(), deltaprimeminus.data(), xplusprimeminus.data()); 125 | JFD.col(i) = (xplusprimeplus - xplusprimeminus)/2e-8; 126 | } 127 | 128 | // std::cout << "J\n" << J << "\n"; 129 | // std::cout << "JFD\n" << JFD << "\n"; 130 | EXPECT_LE((J - JFD).array().abs().sum(), 1e-5); 131 | 132 | } 133 | 134 | TEST(Attitude3d, AverageAttitude) 135 | { 136 | int numObs = 1000; 137 | int noise_level = 1e-2; 138 | quat::Quat x = quat::Quat::Random(); 139 | quat::Quat xhat = quat::Quat::Identity(); 140 | 141 | Problem problem; 142 | problem.AddParameterBlock(xhat.data(), 4, new QuatParam()); 143 | 144 | for (int i = 0; i < numObs; i++) 145 | { 146 | quat::Quat sample = x + Vector3d::Random()*noise_level; 147 | problem.AddResidualBlock(new QuatFactor(sample.data()), NULL, xhat.data()); 148 | } 149 | 150 | Solver::Options options; 151 | options.max_num_iterations = 25; 152 | options.linear_solver_type = ceres::DENSE_QR; 153 | options.minimizer_progress_to_stdout = false; 154 | 155 | Solver::Summary summary; 156 | ceres::Solve(options, &problem, &summary); 157 | double error = (xhat - x).array().abs().sum(); 158 | if (error > 1e-8) 159 | int debug = 1; 160 | EXPECT_LE(error, 1e-8); 161 | } 162 | 163 | TEST(Attitude3d, AverageAttitudeAutoDiff) 164 | { 165 | int numObs = 1000; 166 | int noise_level = 1e-2; 167 | quat::Quat x = quat::Quat::Random(); 168 | quat::Quat xhat = quat::Quat::Identity(); 169 | 170 | Problem problem; 171 | problem.AddParameterBlock(xhat.data(), 4, new QuatADParam()); 172 | 173 | for (int i = 0; i < numObs; i++) 174 | { 175 | quat::Quat sample = x + Vector3d::Random()*noise_level; 176 | problem.AddResidualBlock(new QuatFactorAD(new QuatFunctor(sample.data())), NULL, xhat.data()); 177 | } 178 | 179 | Solver::Options options; 180 | options.max_num_iterations = 25; 181 | options.linear_solver_type = ceres::DENSE_QR; 182 | options.minimizer_progress_to_stdout = false; 183 | 184 | Solver::Summary summary; 185 | ceres::Solve(options, &problem, &summary); 186 | double error = (xhat - x).array().abs().sum(); 187 | if (error > 1e-8) 188 | int debug = 1; 189 | EXPECT_LE(error, 1e-8); 190 | } 191 | -------------------------------------------------------------------------------- /src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "geometry/xform.h" 5 | #include "geometry/cam.h" 6 | #include "utils/jac.h" 7 | #include "factors/camera.h" 8 | #include "factors/SE3.h" 9 | #include "multirotor_sim/utils.h" 10 | 11 | using namespace Eigen; 12 | using namespace ceres; 13 | using namespace xform; 14 | 15 | TEST(Camera, Proj_InvProj) 16 | { 17 | Vector2d focal_len{250.0, 250.0}; 18 | Vector2d cam_center{320.0, 240.0}; 19 | Vector2d img_size{640, 480}; 20 | Vector5d distortion = (Vector5d() << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0).finished(); 21 | double s = 0.0; 22 | Camera cam(focal_len, cam_center, distortion, s, img_size); 23 | 24 | Vector3d pt{1.0, 0.5, 2.0}; 25 | double depth = pt.norm(); 26 | Vector2d pix; 27 | Vector3d pt2; 28 | 29 | cam.proj(pt, pix); 30 | cam.invProj(pix, depth, pt2); 31 | 32 | EXPECT_NEAR(pt.x(), pt2.x(), 1e-6); 33 | EXPECT_NEAR(pt.y(), pt2.y(), 1e-6); 34 | EXPECT_NEAR(pt.z(), pt2.z(), 1e-6); 35 | } 36 | 37 | TEST(Camera, Distort_UnDistort) 38 | { 39 | Vector2d focal_len{250.0, 250.0}; 40 | Vector2d cam_center{320.0, 240.0}; 41 | Vector2d img_size{640, 480}; 42 | Vector5d distortion = (Vector5d() << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0).finished(); 43 | double s = 0.0; 44 | Camera cam(focal_len, cam_center, distortion, s, img_size); 45 | 46 | Vector2d pix_d, pix_u; 47 | Vector2d pix_d2, pi_d2; 48 | Vector3d pt{1.0, 0.5, 2.0}; 49 | 50 | cam.proj(pt, pix_d); 51 | Vector2d pi_d, pi_u; 52 | 53 | cam.pix2intrinsic(pix_d, pi_d); 54 | cam.Distort(pi_d, pi_u); 55 | cam.unDistort(pi_u, pi_d2); 56 | cam.intrinsic2pix(pi_d2, pix_d2); 57 | 58 | EXPECT_NEAR(pix_d2.x(), pix_d.x(), 1e-3); 59 | EXPECT_NEAR(pix_d2.y(), pix_d.y(), 1e-3); 60 | } 61 | 62 | TEST(Camera, UnDistort_Distort) 63 | { 64 | Vector2d focal_len{250.0, 250.0}; 65 | Vector2d cam_center{320.0, 240.0}; 66 | Vector2d img_size{640, 480}; 67 | Vector5d distortion = (Vector5d() << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0).finished(); 68 | double s = 0.0; 69 | Camera cam(focal_len, cam_center, distortion, s, img_size); 70 | 71 | Vector2d pix_d, pix_u; 72 | Vector2d pix_u2, pi_u2; 73 | Vector3d pt{1.0, 0.5, 2.0}; 74 | 75 | cam.proj(pt, pix_d); 76 | Vector2d pi_d, pi_u; 77 | 78 | cam.pix2intrinsic(pix_u, pi_u); 79 | cam.unDistort(pi_u, pi_d); 80 | cam.Distort(pi_d, pi_u2); 81 | cam.intrinsic2pix(pi_u2, pix_u2); 82 | 83 | EXPECT_NEAR(pix_u2.x(), pix_u.x(), 1e-3); 84 | EXPECT_NEAR(pix_u2.y(), pix_u.y(), 1e-3); 85 | } 86 | 87 | TEST (Camera, DistortJacobian) 88 | { 89 | Vector2d focal_len{250.0, 250.0}; 90 | Vector2d cam_center{320.0, 240.0}; 91 | Vector2d img_size{640, 480}; 92 | Vector5d distortion = (Vector5d() << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0).finished(); 93 | double s = 0.0; 94 | Camera cam(focal_len, cam_center, distortion, s, img_size); 95 | 96 | Matrix2d JA, JFD; 97 | Vector2d x0{0.1, 0.5}; 98 | 99 | auto fun = [cam](const MatrixXd& x_u) 100 | { 101 | Vector2d x_d; 102 | cam.unDistort(x_u, x_d); 103 | return x_d; 104 | }; 105 | 106 | JFD = calc_jac(fun, x0); 107 | cam.distortJac(x0, JA); 108 | // cout << "JA\n" << JA << endl;/ 109 | // cout << "JFD\n" << JFD << endl; 110 | } 111 | 112 | TEST (Camera, Intrinsics_Calibration) 113 | { 114 | MatrixXd landmarks; 115 | landmarks.resize(3, 100); 116 | for (int i = 0; i < 10; i++) 117 | { 118 | for (int j = 0; j < 10; j++) 119 | { 120 | landmarks.block<3,1>(0,i*10+j) << (i-5)/10.0, (j-5)/10.0, 0; 121 | } 122 | } 123 | 124 | MatrixXd camera_pose; 125 | camera_pose.resize(7, 9); 126 | double deg15 = 15.0*M_PI/180.0; 127 | double deg7 = 7.0*M_PI/180.0; 128 | camera_pose.col(0) = Xformd(Vector3d{0, 0, -1}, Quatd::Identity()).elements(); 129 | camera_pose.col(1) = Xformd(Vector3d{1, 0, -1}, Quatd::from_euler(deg15, 0, 0)).elements(); 130 | camera_pose.col(2) = Xformd(Vector3d{1, 1, -1}, Quatd::from_euler(deg7, -deg7, 0)).elements(); 131 | camera_pose.col(3) = Xformd(Vector3d{0, 1, -1}, Quatd::from_euler(0, -deg15, 0)).elements(); 132 | camera_pose.col(4) = Xformd(Vector3d{-1, 1, -1}, Quatd::from_euler(-deg7, deg7, 0)).elements(); 133 | camera_pose.col(5) = Xformd(Vector3d{-1, 0, -1}, Quatd::from_euler(-deg15, 0, 0)).elements(); 134 | camera_pose.col(6) = Xformd(Vector3d{-1, -1, -1}, Quatd::from_euler(-deg7, deg7, 0)).elements(); 135 | camera_pose.col(7) = Xformd(Vector3d{0, -1, -1}, Quatd::from_euler(0, deg15, 0)).elements(); 136 | camera_pose.col(8) = Xformd(Vector3d{1, -1, -1}, Quatd::from_euler(deg7, deg7, 0)).elements(); 137 | 138 | Vector2d focal_len{250.0, 250.0}; 139 | Vector2d cam_center{320.0, 240.0}; 140 | Vector2d img_size{640, 480}; 141 | Vector5d distortion = (Vector5d() << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0).finished(); 142 | double s = 0.0; 143 | Camera cam(focal_len, cam_center, distortion, s, img_size); 144 | 145 | Vector2d fhat = focal_len + Vector2d::Random()*25; 146 | Vector2d chat = cam_center + Vector2d::Random()*25; 147 | Vector5d dhat = distortion + Vector5d::Random()*1e-3; 148 | double shat = s + (rand() % 1000)*1e-6; 149 | 150 | MatrixXd xhat = camera_pose; 151 | MatrixXd lhat = landmarks; 152 | 153 | Problem problem; 154 | problem.AddParameterBlock(fhat.data(), 2); 155 | problem.AddParameterBlock(chat.data(), 2); 156 | problem.AddParameterBlock(dhat.data(), 5); 157 | problem.AddParameterBlock(&shat, 1); 158 | for (int i = 0; i < 100; i++) 159 | { 160 | problem.AddParameterBlock(lhat.data()+3*i, 3); 161 | problem.SetParameterBlockConstant(lhat.data()+3*i); 162 | } 163 | for (int i = 0; i < 9; i++) 164 | { 165 | problem.AddParameterBlock(xhat.data()+7*i, 7, new XformParamAD()); 166 | problem.SetParameterBlockConstant(xhat.data()+7*i); 167 | } 168 | 169 | Matrix2d cov = Matrix2d::Identity() * 1e-5; 170 | for (int i = 0; i < 9; i++) 171 | { 172 | Xformd x_w2c(camera_pose.col(i)); // transform from world to camera 173 | for (int j = 0; j < 100; j++) 174 | { 175 | Vector2d pix; 176 | cam.proj(x_w2c.transformp(landmarks.col(j)), pix); 177 | 178 | if (cam.check(pix)) 179 | { 180 | problem.AddResidualBlock(new CamFactorAD(new CamFunctor(pix, cov, cam.image_size_)), 181 | NULL, lhat.data()+3*j, xhat.data()+7*i, fhat.data(), chat.data(), &shat, 182 | dhat.data()); 183 | } 184 | } 185 | } 186 | 187 | Solver::Options options; 188 | options.max_num_iterations = 100; 189 | options.num_threads = 6; 190 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 191 | options.minimizer_progress_to_stdout = false; 192 | Solver::Summary summary; 193 | 194 | // cout << "fhat0\t" << fhat.transpose() << endl; 195 | // cout << "chat0\t" << chat.transpose() << endl; 196 | // cout << "dhat0\t" << dhat.transpose() << endl; 197 | // cout << "shat0\t" << shat << endl; 198 | 199 | ceres::Solve(options, &problem, &summary); 200 | // std::cout << summary.FullReport(); 201 | 202 | // cout << "fhatf\t" << fhat.transpose() << " : " << focal_len.transpose() << endl; 203 | // cout << "chatf\t" << chat.transpose() << " : " << cam_center.transpose() << endl; 204 | // cout << "dhatf\t" << dhat.transpose() << " : " << distortion.transpose() << endl; 205 | // cout << "shatf\t" << shat << " : " << s << endl; 206 | 207 | } 208 | -------------------------------------------------------------------------------- /src/carrier_phase.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "geometry/xform.h" 9 | #include "multirotor_sim/simulator.h" 10 | #include "multirotor_sim/controller.h" 11 | #include "multirotor_sim/satellite.h" 12 | #include "multirotor_sim/estimator_wrapper.h" 13 | #include "utils/logger.h" 14 | #include "test_common.h" 15 | #include "utils/trajectory_sim.h" 16 | 17 | #include "factors/pseudorange.h" 18 | #include "factors/SE3.h" 19 | #include "factors/clock_bias_dynamics.h" 20 | #include "factors/carrier_phase.h" 21 | #include "factors/imu_3d.h" 22 | 23 | class TestCarrierPhase : public ::testing::Test 24 | { 25 | protected: 26 | TestCarrierPhase() : 27 | sat(1, 0) 28 | {} 29 | void SetUp() override 30 | { 31 | time.week = 86400.00 / DateTime::SECONDS_IN_WEEK; 32 | time.tow_sec = 86400.00 - (time.week * DateTime::SECONDS_IN_WEEK); 33 | 34 | eph.sat = 1; 35 | eph.A = 5153.79589081 * 5153.79589081; 36 | eph.toe.week = 93600.0 / DateTime::SECONDS_IN_WEEK; 37 | eph.toe.tow_sec = 93600.0 - (eph.toe.week * DateTime::SECONDS_IN_WEEK); 38 | eph.toes = 93600.0; 39 | eph.deln = 0.465376527657e-08; 40 | eph.M0 = 1.05827953357; 41 | eph.e = 0.00223578442819; 42 | eph.omg = 2.06374037770; 43 | eph.cus = 0.177137553692e-05; 44 | eph.cuc = 0.457651913166e-05; 45 | eph.crs = 88.6875000000; 46 | eph.crc = 344.96875; 47 | eph.cis = -0.856816768646e-07; 48 | eph.cic = 0.651925802231e-07; 49 | eph.idot = 0.342514267094e-09; 50 | eph.i0 = 0.961685061380; 51 | eph.OMG0 = 1.64046615454; 52 | eph.OMGd = -0.856928551657e-08; 53 | sat.addEphemeris(eph); 54 | 55 | } 56 | eph_t eph; 57 | GTime time; 58 | Satellite sat; 59 | }; 60 | 61 | TEST_F(TestCarrierPhase, CheckResidualAtInit) 62 | { 63 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 64 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 65 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 66 | 67 | double dt = 2.5; 68 | 69 | GTime t0 = time; 70 | GTime t1 = t0 + dt; 71 | 72 | Vector3d z0, z1; 73 | sat.computeMeasurement(t0, rec_pos, Vector3d::Zero(), Vector2d::Zero(), z0); 74 | sat.computeMeasurement(t1, rec_pos, Vector3d::Zero(), Vector2d::Zero(), z1); 75 | 76 | double dPhi = z1(2) - z0(2); 77 | CarrierPhaseFunctor phase_factor(t0, t1, dPhi, sat, rec_pos, 1.0); 78 | 79 | Xformd x = Xformd::Identity(); 80 | Vector2d clk = Vector2d::Zero(); 81 | double res = NAN; 82 | 83 | phase_factor(x.data(), x.data(), clk.data(), clk.data(), x_e2n.data(), &res); 84 | 85 | EXPECT_NEAR(res, 0.0, 1e-2); 86 | } 87 | 88 | TEST_F(TestCarrierPhase, CheckResidualAfterMovingAtInit) 89 | { 90 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 91 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 92 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 93 | 94 | Xformd x0 = Xformd::Identity(); 95 | Xformd x1 = Xformd::Identity(); 96 | x1.t() += Vector3d{10, 0, 0}; 97 | 98 | double dt = 2.5; 99 | 100 | GTime t0 = time; 101 | GTime t1 = t0 + dt; 102 | 103 | Vector3d z0, z1; 104 | sat.computeMeasurement(t0, WSG84::ned2ecef(x_e2n, x0.t()), Vector3d::Zero(), Vector2d::Zero(), z0); 105 | sat.computeMeasurement(t1, WSG84::ned2ecef(x_e2n, x1.t()), Vector3d::Zero(), Vector2d::Zero(), z1); 106 | 107 | double dPhi = z1(2) - z0(2); 108 | CarrierPhaseFunctor phase_factor(t0, t1, dPhi, sat, rec_pos, 1.0); 109 | 110 | Vector2d clk = Vector2d::Zero(); 111 | double res = NAN; 112 | 113 | phase_factor(x0.data(), x1.data(), clk.data(), clk.data(), x_e2n.data(), &res); 114 | 115 | EXPECT_NEAR(res, 0.0, 1e-2); 116 | } 117 | 118 | TEST (CarrierPhase, Trajectory) 119 | { 120 | TrajectorySim a(raw_gps_yaml_file()); 121 | 122 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 123 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 124 | a.addNodeCB([&a](){a.addCarrierPhaseFactors();}); 125 | 126 | a.run(); 127 | a.solve(); 128 | a.log("/tmp/ceres_sandbox/CarrierPhase.Trajectory.log"); 129 | 130 | ASSERT_LE(a.final_error(), a.error0); 131 | } 132 | 133 | TEST (CarrierPhase, ImuTrajectory) 134 | { 135 | TrajectorySim a(raw_gps_yaml_file()); 136 | a.fix_origin(); 137 | 138 | a.addNodeCB([&a](){a.addImuFactor();}); 139 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 140 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 141 | a.addNodeCB([&a](){a.addCarrierPhaseFactors();}); 142 | 143 | a.run(); 144 | a.solve(); 145 | a.log("/tmp/ceres_sandbox/CarrierPhase.ImuTrajectory.log"); 146 | 147 | ASSERT_LE(a.final_error(), a.error0); 148 | } 149 | 150 | 151 | -------------------------------------------------------------------------------- /src/control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | #include "factors/dynamics_1d.h" 9 | #include "factors/dynamics_3d.h" 10 | 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | TEST (Control, Robot1d_OptimizeTrajectorySingleWindow) 15 | { 16 | double x0[2] = {0, 0}; 17 | double xf[2] = {1, 0}; 18 | 19 | const int N = 500 ; 20 | double tmax = 1.0; 21 | double dt = tmax/(double)N; 22 | 23 | MatrixXd x; 24 | MatrixXd u; 25 | x.setZero(2, N); 26 | u.setZero(1, N); 27 | 28 | ceres::Problem problem; 29 | 30 | double v_init = (xf[0] - x0[0])/tmax; 31 | 32 | for (int i = 0; i < N; i++) 33 | { 34 | // add parameter blocks 35 | problem.AddParameterBlock(x.data() + 2*i, 2); 36 | problem.AddParameterBlock(u.data() + i, 1); 37 | } 38 | 39 | // pin initial pose (constraint) 40 | problem.AddResidualBlock(new PosVel1DFactorAD(new PosVel1DFunctor(x0[0], x0[1])), NULL, x.data()); 41 | x.col(0) << x0[0], v_init; 42 | u(0) = 0; 43 | 44 | problem.AddResidualBlock(new Input1DFactorAD(new Input1DFunctor()), NULL, u.data()); 45 | for (int i = 1; i < N; i++) 46 | { 47 | // dynamics constraint 48 | problem.AddResidualBlock(new Dyn1dFactorAD(new Dyn1DFunctor(dt)), NULL, 49 | x.data()+(i-1)*2, x.data()+i*2, u.data()+i-1, u.data()+i); 50 | // input cost 51 | problem.AddResidualBlock(new Input1DFactorAD(new Input1DFunctor()), NULL, u.data()+i); 52 | x.col(i) << v_init*i*dt, v_init; 53 | } 54 | 55 | // pin final pose (constraint) 56 | problem.AddResidualBlock(new PosVel1DFactorAD(new PosVel1DFunctor(xf[0], x0[1])), NULL, x.data()+2*(N-1)); 57 | x.col(N-1) << xf[0], v_init; 58 | 59 | ceres::Solver::Options options; 60 | options.max_num_iterations = 1000; 61 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 62 | options.minimizer_progress_to_stdout = false; 63 | ceres::Solver::Summary summary; 64 | std::ofstream traj0_file("/tmp/ceres_sandbox/1d_trajectory_states_init.bin"); 65 | traj0_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 66 | traj0_file.close(); 67 | 68 | ceres::Solve(options, &problem, &summary); 69 | 70 | std::ofstream traj_file("/tmp/ceres_sandbox/1d_trajectory_states.bin"); 71 | std::ofstream input_file("/tmp/ceres_sandbox/1d_trajectory_input.bin"); 72 | traj_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 73 | input_file.write((char*)u.data(), sizeof(double)*u.rows()*u.cols()); 74 | traj_file.close(); 75 | input_file.close(); 76 | } 77 | 78 | TEST (Control, Robot1d_OptimizeTrajectoryMultiWindow) 79 | { 80 | 81 | const int W = 8; // Number of windows 82 | const int K = 500; // collocation Points per window 83 | double tmax = 1.0; 84 | double dt = tmax/(double)K; 85 | 86 | Matrix x_desired; 87 | x_desired << 0, 1, 0, 3, -1, 0, -4, 0, 5, 88 | 0, -6, 0, 9, 0, -9, 0, 15, 0; 89 | 90 | MatrixXd x; 91 | MatrixXd u; 92 | x.setZero(2, K*W+1); 93 | u.setZero(1, K*W+1); 94 | 95 | ceres::Problem problem; 96 | 97 | for (int i = 0; i <= K*W; i++) 98 | { 99 | // add parameter blocks 100 | problem.AddParameterBlock(x.data() + 2*i, 2); 101 | problem.AddParameterBlock(u.data() + i, 1); 102 | } 103 | 104 | for (int w = 0; w < W; w++) 105 | { 106 | 107 | Vector2d x0 = x_desired.col(w); 108 | Vector2d xf = x_desired.col(w+1); 109 | 110 | double v_init = (xf[0] - x0[0])/tmax; 111 | 112 | 113 | // pin initial pose (constraint) 114 | problem.AddResidualBlock(new Input1DFactorAD(new Input1DFunctor()), NULL, u.data()); 115 | for (int i = 0; i < K; i++) 116 | { 117 | int id = i + K*w; 118 | if (i == 0) 119 | { 120 | problem.AddResidualBlock(new PosVel1DFactorAD( 121 | new PosVel1DFunctor(x0[0], x0[1])), NULL, x.data() + id*2); 122 | } 123 | 124 | if (id > 0) 125 | { 126 | // dynamics constraint 127 | problem.AddResidualBlock(new Dyn1dFactorAD(new Dyn1DFunctor(dt)), NULL, 128 | x.data()+(id-1)*2, x.data()+id*2, u.data()+id-1, u.data()+id); 129 | } 130 | // input cost 131 | problem.AddResidualBlock(new Input1DFactorAD(new Input1DFunctor()), NULL, u.data()+id); 132 | x.col(id) << x0[0] + v_init*i*dt, v_init; 133 | u(id) = 0; 134 | } 135 | } 136 | 137 | // pin final pose (constraint) 138 | int id = K*W; 139 | Vector2d xf = x_desired.rightCols(1); 140 | problem.AddResidualBlock(new Dyn1dFactorAD(new Dyn1DFunctor(dt)), NULL, 141 | x.data()+(id-1)*2, x.data()+id*2, u.data()+id-1, u.data()+id); 142 | problem.AddResidualBlock(new Input1DFactorAD(new Input1DFunctor()), NULL, u.data()+id); 143 | problem.AddResidualBlock(new PosVel1DFactorAD( 144 | new PosVel1DFunctor(xf[0], xf[1])), NULL, x.data() + id*2); 145 | x.col(id) << xf[0], 1.0; 146 | 147 | ceres::Solver::Options options; 148 | options.max_num_iterations = 1000; 149 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 150 | options.minimizer_progress_to_stdout = false; 151 | ceres::Solver::Summary summary; 152 | std::ofstream traj0_file("/tmp/ceres_sandbox/1d_trajectory_multi_states_init.bin"); 153 | traj0_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 154 | traj0_file.close(); 155 | 156 | ceres::Solve(options, &problem, &summary); 157 | 158 | std::ofstream traj_file("/tmp/ceres_sandbox/1d_trajectory_multi_states.bin"); 159 | std::ofstream input_file("/tmp/ceres_sandbox/1d_trajectory_multi_input.bin"); 160 | traj_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 161 | input_file.write((char*)u.data(), sizeof(double)*u.rows()*u.cols()); 162 | traj_file.close(); 163 | input_file.close(); 164 | } 165 | 166 | TEST (DISABLED_Control, MultirotorSingleStep) 167 | { 168 | typedef Matrix Vec10; 169 | Vec10 x0 = (Vec10() << 0, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(); 170 | Vec10 xf = (Vec10() << 1, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(); 171 | 172 | Matrix4d R; 173 | R << 1e6, 0, 0, 0, 174 | 0, 1e6, 0, 0, 175 | 0, 0, 1e6, 0, 176 | 0, 0, 0, 1e6; 177 | double dyn_cost = 1e6; 178 | MatrixXd x, u; 179 | x.setZero(10, 2); 180 | u.setZero(4, 2); 181 | double dt = 0.1; 182 | u.row(3).setConstant(0.5); 183 | 184 | ceres::Problem problem; 185 | 186 | problem.AddParameterBlock(x.data(), 10, new Dynamics3DPlusParamAD()); 187 | problem.AddParameterBlock(x.data()+10, 10, new Dynamics3DPlusParamAD()); 188 | problem.AddParameterBlock(u.data(), 4); 189 | 190 | x.col(0) = x0; 191 | x.col(1) = xf; 192 | 193 | problem.AddResidualBlock(new PosVel3DFactorAD( 194 | new PosVel3DFunctor(x0.segment<3>(0), 0, dyn_cost)), NULL, x.data()); 195 | problem.AddResidualBlock(new PosVel3DFactorAD( 196 | new PosVel3DFunctor(xf.segment<3>(0), 0, dyn_cost)), NULL, x.data()+10); 197 | problem.AddResidualBlock(new Dyn3DFactorAD(new Dyn3dFunctor(dt, dyn_cost)), NULL, 198 | x.data(), x.data()+10, u.data(), u.data()+4); 199 | problem.AddResidualBlock(new Input3DFactorAD(new Input3DFunctor(R)), NULL, u.data()); 200 | problem.AddResidualBlock(new Input3DFactorAD(new Input3DFunctor(R)), NULL, u.data()+4); 201 | 202 | ceres::Solver::Options options; 203 | options.max_num_iterations = 1000; 204 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 205 | options.minimizer_progress_to_stdout = true; 206 | ceres::Solver::Summary summary; 207 | ceres::Solve(options, &problem, &summary); 208 | } 209 | 210 | TEST (DISABLED_Control, Multirotor3d_OptimizeTrajectorySingleWindow) 211 | { 212 | typedef Matrix Vec10; 213 | Vec10 x0 = (Vec10() << 0, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(); 214 | Vec10 xf = (Vec10() << 0.2, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(); 215 | 216 | Matrix4d R; 217 | R << 1, 0, 0, 0, 218 | 0, 1, 0, 0, 219 | 0, 0, 1, 0, 220 | 0, 0, 0, 2; 221 | double dyn_cost = 1e6; 222 | double pos_cost = 1e6; 223 | 224 | const int N = 100; 225 | double tmax = 1.0; 226 | double dt = tmax/(double)(N-1); 227 | 228 | MatrixXd x; 229 | MatrixXd u; 230 | x.setZero(10, N); 231 | u.setZero(4, N); 232 | u.row(3).setConstant(0.5); 233 | 234 | ceres::Problem problem; 235 | 236 | // double v_init = (xf.segment<3>(0) - x0.segment<3>(0)).norm()/tmax; 237 | 238 | for (int i = 0; i < N; i++) 239 | { 240 | // add parameter blocks 241 | problem.AddParameterBlock(x.data() + 10*i, 10, new Dynamics3DPlusParamAD()); 242 | problem.AddParameterBlock(u.data() + 4*i, 4); 243 | problem.SetParameterUpperBound(u.data() + 4*i, 3, 1.0); 244 | problem.SetParameterLowerBound(u.data() + 4*i, 3, 0.0); 245 | } 246 | // problem.SetParameterBlockConstant(x.data()); 247 | // problem.SetParameterBlockConstant(x.data() + (N-1)*10); 248 | 249 | // pin initial pose (constraint) 250 | problem.AddResidualBlock(new PosVel3DFactorAD( 251 | new PosVel3DFunctor(x0.segment<3>(0), 0, pos_cost)), NULL, x.data()); 252 | x.col(0) = x0; 253 | 254 | problem.AddResidualBlock(new Input3DFactorAD(new Input3DFunctor(R)), NULL, u.data()); 255 | for (int i = 1; i < N; i++) 256 | { 257 | // dynamics constraint 258 | problem.AddResidualBlock(new Dyn3DFactorAD(new Dyn3dFunctor(dt, dyn_cost)), NULL, 259 | x.data()+(i-1)*10, x.data()+i*10, u.data()+(i-1)*4, u.data()+i*4); 260 | // input cost 261 | problem.AddResidualBlock(new Input3DFactorAD(new Input3DFunctor(R)), NULL, u.data()+4*i); 262 | x.col(i) = x0; 263 | x.block<3,1>(0,i) = (xf.segment<3>(0) - x0.segment<3>(0))/tmax * (double)i*dt + x0.segment<3>(0); 264 | // x(3,i) = 0; 265 | } 266 | 267 | // pin final pose (constraint) 268 | Vector3d pf = xf.segment<3>(0); 269 | problem.AddResidualBlock(new PosVel3DFactorAD( 270 | new PosVel3DFunctor(pf, 0.0, pos_cost)), NULL, x.data()+10*(N-1)); 271 | x.col(N-1) << xf; 272 | 273 | ceres::Solver::Options options; 274 | options.max_num_iterations = 1000; 275 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 276 | options.minimizer_progress_to_stdout = true; 277 | ceres::Solver::Summary summary; 278 | std::ofstream traj0_file("/tmp/ceres_sandbox/3d_trajectory_states_init.bin"); 279 | traj0_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 280 | traj0_file.close(); 281 | // cout << "x0 = \n" << x << endl; 282 | 283 | for (int i = 0; i < 3; i++) 284 | { 285 | ceres::Solve(options, &problem, &summary); 286 | dyn_cost *= 10.0; 287 | pos_cost *= 10.0; 288 | } 289 | cout << summary.FullReport() << endl; 290 | 291 | // cout << "xf = \n" << x << endl; 292 | std::ofstream traj_file("/tmp/ceres_sandbox/3d_trajectory_states.bin"); 293 | std::ofstream input_file("/tmp/ceres_sandbox/3d_trajectory_input.bin"); 294 | traj_file.write((char*)x.data(), sizeof(double)*x.rows()*x.cols()); 295 | input_file.write((char*)u.data(), sizeof(double)*u.rows()*u.cols()); 296 | traj_file.close(); 297 | input_file.close(); 298 | } 299 | -------------------------------------------------------------------------------- /src/dev_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "utils/logger.h" 10 | #include "utils/trajectory_sim.h" 11 | #include "test_common.h" 12 | 13 | using namespace multirotor_sim; 14 | using namespace Eigen; 15 | using namespace xform; 16 | 17 | TEST (CarrierPhase, Trajectory) 18 | { 19 | TrajectorySim a(raw_gps_yaml_file()); 20 | 21 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 22 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 23 | a.addNodeCB([&a](){a.addCarrierPhaseFactors();}); 24 | 25 | a.run(); 26 | a.solve(); 27 | a.log("/tmp/ceres_sandbox/CarrierPhase.Trajectory.log"); 28 | 29 | ASSERT_LE(a.final_error(), a.error0); 30 | } 31 | 32 | TEST (CarrierPhase, ImuTrajectory) 33 | { 34 | TrajectorySim a(raw_gps_yaml_file()); 35 | a.fix_origin(); 36 | 37 | a.addNodeCB([&a](){a.addImuFactor();}); 38 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 39 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 40 | a.addNodeCB([&a](){a.addCarrierPhaseFactors();}); 41 | 42 | a.run(); 43 | a.solve(); 44 | a.log("/tmp/ceres_sandbox/CarrierPhase.ImuTrajectory.log"); 45 | 46 | ASSERT_LE(a.final_error(), a.error0); 47 | } 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/imu1d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "gtest/gtest.h" 8 | 9 | #include "factors/range_1d.h" 10 | 11 | #include "utils/robot1d.h" 12 | #include "factors/imu_1d.h" 13 | #include "utils/jac.h" 14 | 15 | using namespace ceres; 16 | using namespace Eigen; 17 | using namespace std; 18 | 19 | #define NUM_ITERS 1 20 | 21 | TEST(Imu1D, 1DRobotSingleWindow) 22 | { 23 | double ba = 10.0; 24 | double bahat = 0.00; 25 | 26 | double Q = 1e-6; 27 | 28 | Robot1D Robot(ba, Q); 29 | Robot.waypoints_ = {3, 0, 3, 0}; 30 | int window_size = 50; 31 | double dt = 0.01; 32 | 33 | Problem problem; 34 | 35 | Eigen::Matrix xhat; 36 | Eigen::Matrix x; 37 | 38 | 39 | // Initialize the Graph 40 | xhat(0,0) = Robot.xhat_; 41 | xhat(1,0) = Robot.vhat_; 42 | x(0,0) = Robot.x_; 43 | x(1,0) = Robot.v_; 44 | 45 | // Tie the graph to the origin 46 | problem.AddParameterBlock(xhat.data(), 2); 47 | problem.SetParameterBlockConstant(xhat.data()); 48 | 49 | Imu1DFunctor *IMUFactor = new Imu1DFunctor(Robot.t_, bahat, Q); 50 | while (Robot.t_ < window_size*dt) 51 | { 52 | Robot.step(dt); 53 | IMUFactor->integrate(Robot.t_, Robot.ahat_); 54 | } 55 | 56 | // Save the actual state 57 | x(0,1) = Robot.x_; 58 | x(1,1) = Robot.v_; 59 | 60 | // Guess at next state 61 | xhat.col(1) = IMUFactor->estimate_xj(xhat.col(0)); 62 | IMUFactor->finished(); // Calculate the Information matrix in preparation for optimization 63 | 64 | // Add preintegrated IMU 65 | problem.AddParameterBlock(xhat.data()+2, 2); 66 | problem.AddResidualBlock(new Imu1DFactorAD(IMUFactor), NULL, xhat.data(), xhat.data()+2, &bahat); 67 | 68 | // Add measurement of final pose 69 | Vector2d zj = x.col(1); 70 | Matrix2d pose_cov = (Matrix2d() << 1e-8, 0, 0, 1e-8).finished(); 71 | problem.AddResidualBlock(new Pose1DFactor(zj, pose_cov), NULL, xhat.data()+2); 72 | 73 | 74 | Solver::Options options; 75 | options.max_num_iterations = 100; 76 | options.linear_solver_type = ceres::DENSE_QR; 77 | options.minimizer_progress_to_stdout = false; 78 | 79 | Solver::Summary summary; 80 | 81 | // cout << "xhat0: \n" << xhat << endl; 82 | // cout << "bahat0 : " << bahat << endl; 83 | // cout << "e0 : " << (x - xhat).array().abs().sum() << endl; 84 | ceres::Solve(options, &problem, &summary); 85 | // cout << "x: \n" << x < xhat; 111 | Eigen::Matrix x; 112 | 113 | 114 | // Initialize the Graph 115 | xhat(0,0) = Robot.xhat_; 116 | xhat(1,0) = Robot.vhat_; 117 | x(0,0) = Robot.x_; 118 | x(1,0) = Robot.v_; 119 | 120 | // Tie the graph to the origin 121 | problem.AddParameterBlock(xhat.data(), 2); 122 | problem.SetParameterBlockConstant(xhat.data()); 123 | 124 | for (int i = 1; i < num_windows; i++) 125 | { 126 | Imu1DFunctor *IMUFactor = new Imu1DFunctor(Robot.t_, bahat, Q); 127 | while (Robot.t_ < i*window_size*dt) 128 | { 129 | Robot.step(dt); 130 | IMUFactor->integrate(Robot.t_, Robot.ahat_); 131 | } 132 | 133 | // Save the actual state 134 | x(0,i) = Robot.x_; 135 | x(1,i) = Robot.v_; 136 | 137 | // Guess at next state 138 | xhat.col(i) = IMUFactor->estimate_xj(xhat.col(i-1)); 139 | IMUFactor->finished(); // Calculate the Information matrix in preparation for optimization 140 | 141 | // Add preintegrated IMU 142 | problem.AddParameterBlock(xhat.data()+2*i, 2); 143 | problem.AddResidualBlock(new Imu1DFactorAD(IMUFactor), NULL, xhat.data()+2*(i-1), xhat.data()+2*i, &bahat); 144 | } 145 | 146 | // Add measurement of final pose 147 | Vector2d zj = x.col(num_windows-1); 148 | Matrix2d pose_cov = (Matrix2d() << 1e-8, 0, 0, 1e-8).finished(); 149 | problem.AddResidualBlock(new Pose1DFactor(zj, pose_cov), NULL, xhat.data()+2*(num_windows-1)); 150 | 151 | 152 | Solver::Options options; 153 | options.max_num_iterations = 100; 154 | options.linear_solver_type = ceres::DENSE_QR; 155 | options.minimizer_progress_to_stdout = false; 156 | 157 | Solver::Summary summary; 158 | 159 | // cout << "xhat0: \n" << xhat << endl; 160 | // cout << "bahat0 : " << bahat << endl; 161 | // cout << "e0 : " << (x - xhat).array().square().sum()/ << endl; 162 | ceres::Solve(options, &problem, &summary); 163 | // cout << "x: \n" << x < landmarks = (Eigen::Matrix() << -20, -15, -10, -5, 5, 10, 15, 20).finished(); 188 | Eigen::Matrix lhat = (Eigen::Matrix() << -21, -13, -8, -5, 3, 12, 16, 23).finished(); 189 | 190 | double rvar = 1e-2; 191 | std::default_random_engine gen; 192 | std::normal_distribution normal(0.0,1.0); 193 | 194 | Problem problem; 195 | 196 | Eigen::Matrix xhat; 197 | Eigen::Matrix x; 198 | 199 | 200 | // Initialize the Graph 201 | xhat(0,0) = Robot.xhat_; 202 | xhat(1,0) = Robot.vhat_; 203 | x(0,0) = Robot.x_; 204 | x(1,0) = Robot.v_; 205 | 206 | // Tie the graph to the origin 207 | problem.AddParameterBlock(xhat.data(), 2); 208 | problem.SetParameterBlockConstant(xhat.data()); 209 | 210 | for (int win = 1; win < num_windows; win++) 211 | { 212 | Imu1DFunctor *IMUFactor = new Imu1DFunctor(Robot.t_, bahat, Q); 213 | while (Robot.t_ < win*window_size*dt) 214 | { 215 | Robot.step(dt); 216 | IMUFactor->integrate(Robot.t_, Robot.ahat_); 217 | } 218 | 219 | // Save the actual state 220 | x(0, win) = Robot.x_; 221 | x(1, win) = Robot.v_; 222 | 223 | // Guess at next state 224 | xhat.col(win) = IMUFactor->estimate_xj(xhat.col(win-1)); 225 | IMUFactor->finished(); // Calculate the Information matrix in preparation for optimization 226 | 227 | // Add preintegrated IMU 228 | problem.AddParameterBlock(xhat.data()+2*win, 2); 229 | problem.AddResidualBlock(new Imu1DFactorAD(IMUFactor), NULL, xhat.data()+2*(win-1), xhat.data()+2*win, &bahat); 230 | 231 | // Add landmark measurements 232 | for (int l = 0; l < landmarks.size(); l++) 233 | { 234 | double rbar = (landmarks[l] - Robot.x_) + normal(gen)*std::sqrt(rvar); 235 | problem.AddResidualBlock(new RangeVel1DFactor(rbar, rvar), NULL, lhat.data()+l, xhat.data()+2*win); 236 | } 237 | } 238 | 239 | Solver::Options options; 240 | options.max_num_iterations = 100; 241 | options.linear_solver_type = ceres::DENSE_QR; 242 | options.minimizer_progress_to_stdout = false; 243 | 244 | Solver::Summary summary; 245 | 246 | // cout << "xhat0: \n" << xhat << endl; 247 | // cout << "lhat0: \n" << lhat.transpose() << endl; 248 | // cout << "bahat0 : " << bahat << endl; 249 | // cout << "e0 : " << (x - xhat).array().abs().sum() << endl; 250 | ceres::Solve(options, &problem, &summary); 251 | // cout << "x: \n" << x < a; 265 | std::vector dt; 266 | for (int i = 0; i < 1000; i++) 267 | { 268 | a.push_back((rand() % 1000)/1000.0 - 0.5); 269 | dt.push_back(0.01 + ((rand() % 1000)/500.0 - 1.0)/1000.0); 270 | } 271 | Vector2d y0 {0.0, 0.0}; 272 | typedef Eigen::Matrix Matrix1d; 273 | Matrix1d b {(rand() % 1000)/10000.0}; 274 | 275 | auto fun = [a, y0, dt](Matrix1d b) 276 | { 277 | Matrix2d F; 278 | Vector2d A, B; 279 | Vector2d y = y0; 280 | for (int i = 0; i < a.size(); i++) 281 | { 282 | F << 1, dt[i], 0, 1; 283 | A << 0.5*dt[i]*dt[i], dt[i]; 284 | B << -0.5*dt[i]*dt[i], -dt[i]; 285 | y = F*y + A*a[i] + B*b; 286 | } 287 | return y; 288 | }; 289 | 290 | Vector2d J {0, 0}; 291 | Matrix2d F; 292 | Vector2d B; 293 | for (int i = 0; i < a.size(); i++) 294 | { 295 | F << 1, dt[i], 0, 1; 296 | B << -0.5*dt[i]*dt[i], -dt[i]; 297 | J = F * J + B; 298 | } 299 | Vector2d JFD = calc_jac(fun, b); 300 | EXPECT_LE((J-JFD).array().abs().sum(), 1e-6); 301 | } 302 | -------------------------------------------------------------------------------- /src/imu3d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "factors/range_1d.h" 9 | #include "factors/SE3.h" 10 | #include "factors/imu_3d.h" 11 | 12 | #include "geometry/xform.h" 13 | #include "geometry/support.h" 14 | #include "multirotor_sim/simulator.h" 15 | #include "multirotor_sim/controller.h" 16 | #include "multirotor_sim/estimator_wrapper.h" 17 | #include "utils/jac.h" 18 | #include "utils/logger.h" 19 | #include "test_common.h" 20 | 21 | using namespace ceres; 22 | using namespace Eigen; 23 | using namespace std; 24 | using namespace xform; 25 | using namespace multirotor_sim; 26 | 27 | 28 | TEST(Imu3D, compile) 29 | { 30 | Imu3DFunctor imu; 31 | } 32 | 33 | TEST(Imu3D, reset) 34 | { 35 | Imu3DFunctor imu; 36 | Vector6d b0; 37 | b0 << 0, 1, 2, 3, 4, 5; 38 | imu.reset(0, b0); 39 | } 40 | 41 | TEST(Imu3D, Propagation) 42 | { 43 | Simulator multirotor; 44 | multirotor.load(imu_only_sim()); 45 | 46 | typedef Imu3DFunctor IMU; 47 | IMU imu; 48 | Vector6d b0; 49 | b0.setZero(); 50 | Matrix6d cov = Matrix6d::Identity() * 1e-3; 51 | 52 | multirotor.run(); 53 | imu.reset(multirotor.t_, b0); 54 | Xformd x0 = multirotor.state().X; 55 | Vector3d v0 = multirotor.state().v; 56 | 57 | Logger log("/tmp/ceres_sandbox/Imu3D.CheckPropagation.log"); 58 | 59 | Xformd xhat = multirotor.state().X; 60 | Vector3d vhat = multirotor.state().v; 61 | log.log(multirotor.t_); 62 | log.logVectors(xhat.elements(), vhat, multirotor.state().X.elements(), 63 | multirotor.state().v, multirotor.imu()); 64 | 65 | double next_reset = 1.0; 66 | multirotor.tmax_ = 10.0; 67 | while (multirotor.run()) 68 | { 69 | imu.integrate(multirotor.t_, multirotor.imu(), cov); 70 | 71 | if (std::abs(multirotor.t_ - next_reset) <= multirotor.dt_ /2.0) 72 | { 73 | imu.reset(multirotor.t_, b0); 74 | x0 = multirotor.state().X; 75 | v0 = multirotor.state().v; 76 | next_reset += 1.0; 77 | } 78 | 79 | imu.estimateXj(x0.data(), v0.data(), xhat.data(), vhat.data()); 80 | log.log(multirotor.t_); 81 | log.logVectors(xhat.elements(), vhat, multirotor.state().X.elements(), multirotor.state().v, multirotor.imu()); 82 | EXPECT_MAT_NEAR(xhat.t(), multirotor.state().p, 0.076); 83 | EXPECT_QUAT_NEAR(xhat.q(), multirotor.state().q, 0.0033); 84 | EXPECT_MAT_NEAR(vhat, multirotor.state().v, 0.04); 85 | } 86 | } 87 | 88 | 89 | TEST(Imu3D, ErrorStateDynamics) 90 | { 91 | typedef Imu3DFunctor IMU; 92 | IMU y; 93 | IMU yhat; 94 | Vector9d dy; 95 | double t = 0; 96 | const double Tmax = 10.0; 97 | static const double dt = 0.001; 98 | 99 | Vector6d bias; 100 | bias.setZero(); 101 | y.reset(t, bias); 102 | yhat.reset(t, bias); 103 | IMU::boxplus(y.y_, Vector9d::Constant(0.01), yhat.y_); 104 | IMU::boxminus(y.y_, yhat.y_, dy); 105 | 106 | Vector10d y_check; 107 | IMU::boxplus(yhat.y_, dy, y_check); 108 | ASSERT_MAT_NEAR(y.y_, y_check, 1e-8); 109 | 110 | Vector6d u, eta; 111 | u.setZero(); 112 | eta.setZero(); 113 | 114 | std::default_random_engine gen; 115 | std::normal_distribution normal; 116 | 117 | Logger log("/tmp/ceres_sandbox/Imu3d.CheckDynamics.log"); 118 | 119 | 120 | Matrix6d cov = Matrix6d::Identity() * 1e-3; 121 | Vector9d dydot; 122 | log.log(t); 123 | log.logVectors(y.y_, yhat.y_, dy, y_check, u); 124 | for (int i = 0; i < Tmax/dt; i++) 125 | { 126 | u += dt * randomNormal(normal, gen); 127 | t += dt; 128 | y.errorStateDynamics(y.y_, dy, u, eta, dydot); 129 | dy += dydot * dt; 130 | 131 | y.integrate(t, u, cov); 132 | yhat.integrate(t, u, cov); 133 | IMU::boxplus(yhat.y_, dy, y_check); 134 | log.log(t); 135 | log.logVectors(y.y_, yhat.y_, dy, y_check, u); 136 | ASSERT_MAT_NEAR(y.y_, y_check, t > 0.3 ? 5e-6*t*t : 2e-7); 137 | } 138 | } 139 | 140 | 141 | TEST(Imu3D, DynamicsJacobians) 142 | { 143 | Matrix6d cov = Matrix6d::Identity()*1e-3; 144 | 145 | Vector6d b0; 146 | Vector10d y0; 147 | Vector6d u0; 148 | Vector6d eta0; 149 | Vector9d ydot; 150 | Vector9d dy0; 151 | 152 | Matrix9d A; 153 | Eigen::Matrix B; 154 | 155 | for (int i = 0; i < 100; i++) 156 | { 157 | b0.setRandom(); 158 | y0.setRandom(); 159 | y0.segment<4>(6) = Quatd::Random().elements(); 160 | u0.setRandom(); 161 | 162 | eta0.setZero(); 163 | dy0.setZero(); 164 | 165 | Imu3DFunctor f; 166 | f.reset(0, b0); 167 | f.dynamics(y0, u0, ydot, A, B); 168 | Vector9d dy0; 169 | 170 | auto yfun = [&y0, &cov, &b0, &u0, &eta0](const Vector9d& dy) 171 | { 172 | Imu3DFunctor f; 173 | f.reset(0, b0); 174 | Vector9d dydot; 175 | f.errorStateDynamics(y0, dy, u0, eta0, dydot); 176 | return dydot; 177 | }; 178 | auto etafun = [&y0, &cov, &b0, &dy0, &u0](const Vector6d& eta) 179 | { 180 | Imu3DFunctor f; 181 | f.reset(0, b0); 182 | Vector9d dydot; 183 | f.errorStateDynamics(y0, dy0, u0, eta, dydot); 184 | return dydot; 185 | }; 186 | 187 | Matrix9d AFD = calc_jac(yfun, dy0); 188 | Eigen::Matrix BFD = calc_jac(etafun, u0); 189 | 190 | // cout << "A\n" << A << "\nAFD\n" << AFD << "\n\n"; 191 | // cout << "B\n" << B << "\nBFD\n" << BFD << "\n\n"; 192 | 193 | ASSERT_MAT_NEAR(AFD, A, 1e-7); 194 | ASSERT_MAT_NEAR(BFD, B, 1e-7); 195 | } 196 | } 197 | 198 | TEST(Imu3D, BiasJacobians) 199 | { 200 | YAML::Node node; 201 | node = YAML::LoadFile("../lib/multirotor_sim/params/sim_params.yaml"); 202 | ofstream tmp("/tmp/ceres_sandbox.tmp.yaml"); 203 | node["imu_enabled"] = true; 204 | node["alt_enabled"] = false; 205 | node["mocap_enabled"] = false; 206 | node["vo_enabled"] = false; 207 | node["camera_enabled"] = false; 208 | node["gnss_enabled"] = false; 209 | node["raw_gnss_enabled"] = false; 210 | tmp << node; 211 | tmp.close(); 212 | Simulator multirotor(false, 1); 213 | multirotor.load("/tmp/ceres_sandbox.tmp.yaml"); 214 | std::vector> meas; 215 | std::vector t; 216 | multirotor.dt_ = 0.001; 217 | 218 | while (multirotor.t_ < 0.1) 219 | { 220 | multirotor.run(); 221 | meas.push_back(multirotor.imu()); 222 | t.push_back(multirotor.t_); 223 | } 224 | 225 | Matrix6d cov = Matrix6d::Identity()*1e-3; 226 | Vector6d b0; 227 | Eigen::Matrix J, JFD; 228 | 229 | b0.setZero(); 230 | Imu3DFunctor f; 231 | f.reset(0, b0); 232 | Vector10d y0 = f.y_; 233 | for (int i = 0; i < meas.size(); i++) 234 | { 235 | f.integrate(t[i], meas[i], cov); 236 | } 237 | J = f.J_; 238 | 239 | auto fun = [&cov, &meas, &t, &y0](const Vector6d& b0) 240 | { 241 | Imu3DFunctor f; 242 | f.reset(0, b0); 243 | for (int i = 0; i < meas.size(); i++) 244 | { 245 | f.integrate(t[i], meas[i], cov); 246 | } 247 | return f.y_; 248 | }; 249 | auto bm = [](const MatrixXd& x1, const MatrixXd& x2) 250 | { 251 | Vector9d dx; 252 | Imu3DFunctor::boxminus(x1, x2, dx); 253 | return dx; 254 | }; 255 | 256 | JFD = calc_jac(fun, b0, nullptr, nullptr, bm, 1e-5); 257 | // std::cout << "FD:\n" << JFD << "\nA:\n" << J <(0,0) = multirotor.accel_bias_; 270 | b.block<3,1>(3,0) = multirotor.gyro_bias_; 271 | bhat.setZero(); 272 | 273 | Problem problem; 274 | 275 | Eigen::MatrixXd xhat, x; 276 | Eigen::MatrixXd vhat, v; 277 | xhat.resize(7, N+1); 278 | x.resize(7, N+1); 279 | vhat.resize(3, N+1); 280 | v.resize(3, N+1); 281 | 282 | std::normal_distribution normal; 283 | std::default_random_engine rng; 284 | 285 | xhat.col(0) = multirotor.state().X.arr_; 286 | vhat.col(0) = multirotor.dyn_.get_state().v; 287 | x.col(0) = multirotor.state().X.arr_; 288 | v.col(0) = multirotor.dyn_.get_state().v; 289 | 290 | xhat.setZero(); 291 | vhat.setZero(); 292 | xhat.row(3).setConstant(1.0); 293 | 294 | for (int n = 0; n < N; n++) 295 | { 296 | problem.AddParameterBlock(xhat.data()+7*n, 7, new XformParamAD()); 297 | problem.AddParameterBlock(vhat.data()+3*n, 3); 298 | } 299 | problem.AddParameterBlock(bhat.data(), 6); 300 | 301 | 302 | std::vector factors; 303 | factors.push_back(new Imu3DFunctor(0, bhat)); 304 | 305 | int node = 0; 306 | Imu3DFunctor* factor = factors[node]; 307 | auto imu_cb = [&factor](const double& t, const Vector6d& z, const Matrix6d& R) 308 | { 309 | factor->integrate(t, z, R); 310 | }; 311 | 312 | EstimatorWrapper est; 313 | est.register_imu_cb(imu_cb); 314 | multirotor.register_estimator(&est); 315 | 316 | 317 | // Integrate for N frames 318 | std::vector t; 319 | t.push_back(multirotor.t_); 320 | double node_dt = 1.0; 321 | double next_node = node_dt; 322 | Matrix6d P = Matrix6d::Identity() * 0.1; 323 | problem.AddResidualBlock(new XformNodeFactorAD(new XformNodeFunctor(multirotor.state().X.arr_, P)), NULL, xhat.data()); 324 | while (node < N) 325 | { 326 | multirotor.run(); 327 | 328 | if (std::abs(multirotor.t_ - next_node) < node_dt / 2.0) 329 | { 330 | next_node += node_dt; 331 | t.push_back(multirotor.t_); 332 | node += 1; 333 | 334 | // estimate next node pose and velocity with IMU preintegration 335 | factor->estimateXj(xhat.data()+7*(node-1), vhat.data()+3*(node-1), xhat.data()+7*(node), vhat.data()+3*(node)); 336 | // Calculate the Information Matrix of the IMU factor 337 | factor->finished(); 338 | 339 | // Save off True Pose and Velocity for Comparison 340 | x.col(node) = multirotor.state().X.arr_; 341 | v.col(node) = multirotor.state().v; 342 | 343 | xhat.col(node) = (multirotor.state().X + randomNormal(normal, rng)).arr(); 344 | 345 | // Add IMU factor to graph 346 | problem.AddResidualBlock(new Imu3DFactorAD(factor), NULL, 347 | xhat.data()+7*(node-1), xhat.data()+7*node, 348 | vhat.data()+3*(node-1), vhat.data()+3*node, 349 | bhat.data()); 350 | 351 | // Start a new Factor 352 | factors.push_back(new Imu3DFunctor(multirotor.t_, bhat)); 353 | factor = factors[node]; 354 | problem.AddResidualBlock(new XformNodeFactorAD(new XformNodeFunctor(multirotor.state().X.arr_, P)), NULL, xhat.data()+7*(node)); 355 | } 356 | } 357 | 358 | Solver::Options options; 359 | options.max_num_iterations = 100; 360 | options.num_threads = 6; 361 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 362 | options.minimizer_progress_to_stdout = false; 363 | Solver::Summary summary; 364 | Logger log("/tmp/ceres_sandbox/Imu3d.MultiWindow.log"); 365 | 366 | MatrixXd xhat0 = xhat; 367 | MatrixXd vhat0 = vhat; 368 | 369 | // cout << "xhat0\n" << xhat.transpose() << endl; 370 | // cout << "bhat0\n" << bhat.transpose() << endl; 371 | 372 | ceres::Solve(options, &problem, &summary); 373 | double error = (b - bhat).norm(); 374 | 375 | // cout << summary.FullReport(); 376 | // cout << "x\n" << x.transpose() << endl; 377 | // cout << "xhatf\n" << xhat.transpose() << endl; 378 | // cout << "b\n" << b.transpose() << endl; 379 | // cout << "bhat\n" << bhat.transpose() << endl; 380 | // cout << "e " << error << endl; 381 | EXPECT_LE(error, 0.2); 382 | 383 | 384 | for (int i = 0; i <= N; i++) 385 | { 386 | log.log(t[i]); 387 | log.logVectors(xhat0.col(i), vhat0.col(i), xhat.col(i), vhat.col(i), x.col(i), v.col(i)); 388 | } 389 | } 390 | -------------------------------------------------------------------------------- /src/pose.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "gtest/gtest.h" 8 | 9 | #include "geometry/xform.h" 10 | #include "geometry/support.h" 11 | #include "factors/SE3.h" 12 | 13 | #define NUM_ITERS 1 14 | #define PRINT_RESULTS 1 15 | 16 | using namespace ceres; 17 | using namespace Eigen; 18 | using namespace std; 19 | using namespace xform; 20 | 21 | TEST(Pose3D, AveragePoseAutoDiff) 22 | { 23 | for (int j = 0; j < NUM_ITERS; j++) 24 | { 25 | int numObs = 1000; 26 | int noise_level = 1e-2; 27 | Xform x = Xform::Random(); 28 | Vector7d xhat = Xform::Identity().elements(); 29 | 30 | Problem problem; 31 | 32 | problem.AddParameterBlock(xhat.data(), 7, new XformParamAD()); 33 | 34 | for (int i = 0; i < numObs; i++) 35 | { 36 | Vector7d sample = (x + Vector6d::Random()*noise_level).elements(); 37 | problem.AddResidualBlock(new XformFactorAD(new XformFunctor(sample.data())), NULL, xhat.data()); 38 | } 39 | 40 | Solver::Options options; 41 | options.max_num_iterations = 25; 42 | options.linear_solver_type = ceres::DENSE_QR; 43 | options.minimizer_progress_to_stdout = false; 44 | 45 | 46 | Solver::Summary summary; 47 | if (j == -1) 48 | { 49 | std::cout << "x: " << x << "\n"; 50 | std::cout << "xhat:" << Xform(xhat) << "\n"; 51 | } 52 | ceres::Solve(options, &problem, &summary); 53 | if (j == -1) 54 | std::cout << "xhat: " << Xform(xhat) << std::endl; 55 | double error = (Xform(xhat) - x).array().abs().sum(); 56 | EXPECT_LE(error, 1e-8); 57 | } 58 | } 59 | 60 | TEST(Pose3D, GraphSLAM) 61 | { 62 | // Input between nodes 63 | Vector6d u; 64 | u << 1.0, 0.0, 0.0, 0.0, 0.0, 0.1; 65 | 66 | // Odom covariance 67 | Matrix6d odomcov = Matrix6d::Identity(); 68 | odomcov.block<3,3>(0,0) *= 0.5; // delta position noise 69 | odomcov.block<3,3>(3,3) *= 0.001; // delta attitude noise 70 | Matrix6d sqrtodomcov (odomcov.llt().matrixL()); 71 | 72 | // LC covariance 73 | Matrix6d lccov = Matrix6d::Identity(); 74 | lccov.block<3,3>(0,0) *= 0.001; // delta position noise 75 | lccov.block<3,3>(3,3) *= 0.0001; // delta attitude noise 76 | Matrix6d sqrtlccov (lccov.llt().matrixL()); 77 | 78 | // Simulate the robot while building up the graph 79 | const int num_steps = 50; 80 | int num_lc = 12; 81 | 82 | // Raw buffer to hold estimates and truth 83 | Eigen::Matrix x, xhat; 84 | 85 | std::default_random_engine gen; 86 | std::normal_distribution normal(0.0,1.0); 87 | 88 | // Build up loop closures 89 | std::vector> loop_closures; 90 | std::uniform_int_distribution uniform_int(0.0,num_steps); 91 | for (int l = 0; l < num_lc; l++) 92 | { 93 | int from = uniform_int(gen); 94 | int to; 95 | do 96 | { 97 | to = uniform_int(gen); 98 | } while(to == from); 99 | std::vector lc {from, to}; 100 | loop_closures.push_back(lc); 101 | } 102 | 103 | Problem problem; 104 | 105 | // Build up Odometry 106 | for (int k = 0; k < num_steps; k++) 107 | { 108 | if (k == 0) 109 | { 110 | // Starting position 111 | xhat.col(0) = Xform::Identity().elements(); 112 | x.col(0) = Xform::Identity().elements(); 113 | 114 | // Start by pinning the initial pose to the origin 115 | problem.AddParameterBlock(xhat.data(), 7, new XformParamAD()); // Tell ceres that this is a Lie Group 116 | problem.SetParameterBlockConstant(xhat.data()); 117 | } 118 | else 119 | { 120 | int i = k-1; 121 | int j = k; 122 | 123 | // Move forward 124 | Xform xi(x.data()+7*i); 125 | Xform xihat (xhat.data() + 7*i); 126 | Map xj(x.data()+7*j); 127 | Map xjhat(xhat.data()+7*j); 128 | xj = (xi + u).elements(); 129 | 130 | // Create measurement (with noise) 131 | Vector6d noise; 132 | setNormalRandom(noise, normal, gen); 133 | noise = sqrtodomcov*noise; 134 | noise(2) = 0; 135 | noise(3) = 0; 136 | noise(4) = 0; // constrain noise to plane 137 | Vector7d ehat_ij = (Xform::exp(u + noise)).elements(); 138 | xjhat = (xihat * Xform(ehat_ij)).elements(); 139 | 140 | // Add odometry edges to graph 141 | problem.AddParameterBlock(xhat.data() + 7*j, 7, new XformParamAD()); // Tell ceres that this is a lie group 142 | problem.AddResidualBlock(new XformEdgeFactorAD(new XformEdgeFunctor(ehat_ij, odomcov)), NULL, xhat.data()+7*i, xhat.data()+7*j); 143 | } 144 | } 145 | 146 | // Add Loop Closures 147 | for (int l = 0; l < loop_closures.size(); l++) 148 | { 149 | int i = loop_closures[l][0]; 150 | int j = loop_closures[l][1]; 151 | 152 | Xform xi(x.data()+7*i); 153 | Xform xj(x.data()+7*j); 154 | Xform eij = xi.inverse() * (xj); // true transform 155 | 156 | // Create measurement (with noise) 157 | Vector6d noise; 158 | setNormalRandom(noise, normal, gen); 159 | noise = sqrtlccov*noise; 160 | noise(2) = 0; 161 | noise(3) = 0; 162 | noise(4) = 0; // constrain noise to plane 163 | Vector7d ehat_ij = eij.elements(); 164 | 165 | // Add loop closure edge to graph 166 | problem.AddResidualBlock(new XformEdgeFactorAD(new XformEdgeFunctor(ehat_ij, lccov)), NULL, xhat.data()+7*i, xhat.data()+7*j); 167 | } 168 | 169 | 170 | Solver::Options options; 171 | options.max_num_iterations = 25; 172 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 173 | options.minimizer_progress_to_stdout = false; 174 | 175 | 176 | Solver::Summary summary; 177 | ofstream log_file("Pose.GraphSLAM.log", ios::out); 178 | 179 | log_file << "x:\n"; 180 | for (int i = 0; i < num_steps; i++) 181 | { 182 | log_file <<"[" << i << "] - " << Xformd(x.col(i)) << "\n"; 183 | } 184 | 185 | log_file << "xhat0:\n"; 186 | for (int i = 0; i < num_steps; i++) 187 | { 188 | log_file << "[" << i << "] - " << Xformd(xhat.col(i)) << "\t--\t" << 1/6.0 * (Xformd(xhat.col(i)) - Xformd(x.col(i))).array().square().sum() << "\n"; 189 | } 190 | 191 | // Calculate RMSE 192 | double RMSE = 0; 193 | for (int k = 0; k < num_steps; k++) 194 | { 195 | RMSE += 1/6.0 * (Xformd(xhat.col(k)) - Xformd(x.col(k))).array().square().sum(); 196 | } 197 | RMSE /= num_steps; 198 | log_file << "RMSE0: " << RMSE << endl; 199 | 200 | ceres::Solve(options, &problem, &summary); 201 | 202 | log_file << "xhat:\n:"; 203 | for (int i = 0; i < num_steps; i++) 204 | { 205 | log_file << "[" << i << "] - " << Xform(xhat.col(i)) << "\t--\t" << 1/6.0 * (Xformd(xhat.col(i)) - Xformd(x.col(i))).array().square().sum() << "\n"; 206 | } 207 | // Calculate RMSE 208 | RMSE = 0; 209 | for (int k = 0; k < num_steps; k++) 210 | { 211 | RMSE += 1/6.0 * (Xformd(xhat.col(k)) - Xformd(x.col(k))).array().square().sum(); 212 | } 213 | RMSE /= num_steps; 214 | log_file << "RMSEf: " << RMSE << endl; 215 | log_file << endl; 216 | log_file.close(); 217 | 218 | EXPECT_LE(RMSE, 0.5); 219 | } 220 | -------------------------------------------------------------------------------- /src/position1d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "factors/position_1d.h" 8 | #include "factors/position_3d.h" 9 | #include "factors/range_1d.h" 10 | #include "factors/transform_1d.h" 11 | 12 | 13 | using namespace ceres; 14 | using namespace Eigen; 15 | 16 | TEST(Position1D, AveragePoints) 17 | { 18 | double x = 5.0; 19 | int numObs = 1000; 20 | double init_x = 3.0; 21 | double xhat = init_x; 22 | 23 | Problem problem; 24 | 25 | for (int i = 0; i < numObs; i++) 26 | { 27 | double sample = x + (rand() % 1000 - 500)/1000.0; 28 | problem.AddResidualBlock(new Pos1DFactor(sample), NULL, &xhat); 29 | } 30 | 31 | Solver::Options options; 32 | options.max_num_iterations = 25; 33 | options.linear_solver_type = ceres::DENSE_QR; 34 | options.minimizer_progress_to_stdout = false; 35 | 36 | Solver::Summary summary; 37 | ceres::Solve(options, &problem, &summary); 38 | EXPECT_NEAR(xhat, x, 1e-2); 39 | } 40 | 41 | TEST(Position1D, AveragePointsWithParameterBlock) 42 | { 43 | double x = 5.0; 44 | int numObs = 10000; 45 | double init_x = 3.0; 46 | double xhat = init_x; 47 | 48 | Problem problem; 49 | problem.AddParameterBlock(&xhat, 1); 50 | 51 | for (int i = 0; i < numObs; i++) 52 | { 53 | double sample = x + (rand() % 1000 - 500)*1e-4; 54 | problem.AddResidualBlock(new Pos1DFactor(sample), NULL, &xhat); 55 | } 56 | 57 | Solver::Options options; 58 | options.max_num_iterations = 25; 59 | options.linear_solver_type = ceres::DENSE_QR; 60 | options.minimizer_progress_to_stdout = false; 61 | 62 | Solver::Summary summary; 63 | ceres::Solve(options, &problem, &summary); 64 | EXPECT_NEAR(xhat, x, 1e-3); 65 | } 66 | 67 | TEST(Position1D, SLAM) 68 | { 69 | double rvar = 1e-5; 70 | double evar = 1e-3; 71 | 72 | Eigen::Matrix x; 73 | x << 0, 1, 2, 3, 4, 5, 6, 7; 74 | 75 | Eigen::Matrix l; 76 | l << 10.0, 15.0, 13.0; 77 | 78 | Eigen::Matrix lhat; 79 | lhat << 11.0, 12.0, 15.0; 80 | 81 | Eigen::Matrix xhat; 82 | xhat(0) = 0; 83 | 84 | std::default_random_engine gen; 85 | std::normal_distribution normal(0.0,1.0); 86 | 87 | Problem problem; 88 | 89 | // Build up the graph 90 | for (int i = 0; i < x.rows(); i++) 91 | { 92 | if (i > 0) 93 | { 94 | double That = (x(i) - x(i-1)) + normal(gen)*std::sqrt(evar); 95 | xhat(i) = xhat(i-1) + That; 96 | problem.AddResidualBlock(new Transform1DFactor(That, evar), NULL, xhat.data() + i-1, xhat.data() + i); 97 | } 98 | 99 | for (int j = 0; j < l.rows(); j++) 100 | { 101 | double zbar = (l[j] - x[i]) + normal(gen)*std::sqrt(rvar); 102 | problem.AddResidualBlock(new Range1DFactor(zbar, rvar), NULL, lhat.data()+j, xhat.data() + i); 103 | } 104 | 105 | } 106 | 107 | Solver::Options options; 108 | options.max_num_iterations = 25; 109 | options.linear_solver_type = ceres::DENSE_QR; 110 | options.minimizer_progress_to_stdout = false; 111 | 112 | Solver::Summary summary; 113 | // std::cout << "x: " << x.transpose() << std::endl; 114 | // std::cout << " l: " << l.transpose() << std::endl << std::endl; 115 | 116 | double e0 = (x-xhat).norm() + (l-lhat).norm(); 117 | // std::cout << "x0: " << xhat.transpose() << std::endl; 118 | // std::cout << "l0: " << lhat.transpose() << std::endl; 119 | // std::cout << "e0: " << e0 << std::endl << std::endl; 120 | 121 | ceres::Solve(options, &problem, &summary); 122 | double ef = (x-xhat).norm() + (l-lhat).norm(); 123 | // std::cout << "xf: " << xhat.transpose() << std::endl; 124 | // std::cout << "lf: " << lhat.transpose() << std::endl; 125 | // std::cout << "ef: " << ef << std::endl << std::endl; 126 | 127 | EXPECT_LE(ef, 0.17); 128 | } 129 | -------------------------------------------------------------------------------- /src/position3d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "gtest/gtest.h" 3 | #include "Eigen/Dense" 4 | 5 | #include "factors/position_3d.h" 6 | 7 | using namespace ceres; 8 | using namespace Eigen; 9 | 10 | 11 | TEST(Position3D, AveragePoints) 12 | { 13 | Vector3d x = (Vector3d() << 1.0, 2.0, 3.0).finished(); 14 | int numObs = 10000; 15 | Vector3d xhat = (Vector3d() << 4.0, 5.0, -2.0).finished(); 16 | 17 | Problem problem; 18 | 19 | Vector3d sample; 20 | Vector3d mean; 21 | mean.setZero(); 22 | for (int i = 0; i < numObs; i++) 23 | { 24 | sample.setRandom(); 25 | sample *= 5e-2; 26 | sample += x; 27 | mean += sample; 28 | problem.AddResidualBlock(new Pos3DFactor(sample.data()), NULL, xhat.data()); 29 | } 30 | mean /= numObs; 31 | 32 | Solver::Options options; 33 | options.max_num_iterations = 25; 34 | options.linear_solver_type = ceres::DENSE_QR; 35 | options.minimizer_progress_to_stdout = false; 36 | 37 | Solver::Summary summary; 38 | ceres::Solve(options, &problem, &summary); 39 | EXPECT_NEAR(xhat[0], x[0], 1e-3); 40 | EXPECT_NEAR(xhat[1], x[1], 1e-3); 41 | EXPECT_NEAR(xhat[2], x[2], 1e-3); 42 | } 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/pseudorange.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "geometry/xform.h" 9 | #include "multirotor_sim/simulator.h" 10 | #include "multirotor_sim/controller.h" 11 | #include "multirotor_sim/satellite.h" 12 | #include "multirotor_sim/estimator_wrapper.h" 13 | #include "utils/logger.h" 14 | #include "utils/trajectory_sim.h" 15 | 16 | #include "factors/pseudorange.h" 17 | #include "factors/SE3.h" 18 | #include "factors/clock_bias_dynamics.h" 19 | #include "factors/imu_3d.h" 20 | 21 | 22 | using namespace multirotor_sim; 23 | using namespace ceres; 24 | using namespace Eigen; 25 | using namespace xform; 26 | 27 | 28 | TEST (Pseudorange, TestCompile) 29 | { 30 | GTime t; 31 | Vector2d rho; 32 | Satellite sat(1, 0); 33 | Vector3d rec_pos; 34 | Matrix2d cov; 35 | PRangeFunctor prange_factor(t, rho, sat, rec_pos, cov); 36 | } 37 | 38 | class TestPseudorange : public ::testing::Test 39 | { 40 | protected: 41 | TestPseudorange() : 42 | sat(1, 0) 43 | {} 44 | void SetUp() override 45 | { 46 | time.week = 86400.00 / DateTime::SECONDS_IN_WEEK; 47 | time.tow_sec = 86400.00 - (time.week * DateTime::SECONDS_IN_WEEK); 48 | 49 | eph.sat = 1; 50 | eph.A = 5153.79589081 * 5153.79589081; 51 | eph.toe.week = 93600.0 / DateTime::SECONDS_IN_WEEK; 52 | eph.toe.tow_sec = 93600.0 - (eph.toe.week * DateTime::SECONDS_IN_WEEK); 53 | eph.toes = 93600.0; 54 | eph.deln = 0.465376527657e-08; 55 | eph.M0 = 1.05827953357; 56 | eph.e = 0.00223578442819; 57 | eph.omg = 2.06374037770; 58 | eph.cus = 0.177137553692e-05; 59 | eph.cuc = 0.457651913166e-05; 60 | eph.crs = 88.6875000000; 61 | eph.crc = 344.96875; 62 | eph.cis = -0.856816768646e-07; 63 | eph.cic = 0.651925802231e-07; 64 | eph.idot = 0.342514267094e-09; 65 | eph.i0 = 0.961685061380; 66 | eph.OMG0 = 1.64046615454; 67 | eph.OMGd = -0.856928551657e-08; 68 | sat.addEphemeris(eph); 69 | 70 | } 71 | eph_t eph; 72 | GTime time; 73 | Satellite sat; 74 | }; 75 | 76 | TEST_F (TestPseudorange, CheckResidualAtInit) 77 | { 78 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 79 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 80 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 81 | 82 | Vector3d z; 83 | Vector2d rho; 84 | sat.computeMeasurement(time, rec_pos, Vector3d::Zero(), Vector2d::Zero(), z); 85 | rho = z.topRows<2>(); 86 | Matrix2d cov = (Vector2d{3.0, 0.4}).asDiagonal(); 87 | 88 | PRangeFunctor prange_factor(time, rho, sat, rec_pos, cov); 89 | 90 | Xformd x = Xformd::Identity(); 91 | Vector3d v = Vector3d::Zero(); 92 | Vector2d clk = Vector2d::Zero(); 93 | Vector2d res = Vector2d::Zero(); 94 | 95 | prange_factor(x.data(), v.data(), clk.data(), x_e2n.data(), res.data()); 96 | 97 | EXPECT_MAT_NEAR(res, Vector2d::Zero(), 1e-4); 98 | } 99 | 100 | TEST_F (TestPseudorange, CheckResidualAfterMoving) 101 | { 102 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 103 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 104 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 105 | Vector2d clk_bias{1e-8, 1e-6}; 106 | 107 | Vector3d z; 108 | Vector2d rho; 109 | sat.computeMeasurement(time, rec_pos, Vector3d::Zero(), clk_bias, z); 110 | rho = z.topRows<2>(); 111 | Matrix2d cov = (Vector2d{3.0, 0.4}).asDiagonal(); 112 | 113 | PRangeFunctor prange_factor(time, rho, sat, rec_pos, cov); 114 | 115 | Xformd x = Xformd::Identity(); 116 | x.t() << 10, 0, 0; 117 | Vector3d p_ecef = WSG84::ned2ecef(x_e2n, x.t()); 118 | Vector3d znew; 119 | sat.computeMeasurement(time, p_ecef, Vector3d::Zero(), clk_bias, znew); 120 | Vector2d true_res = Vector2d{std::sqrt(1/3.0), std::sqrt(1/0.4)}.asDiagonal() * (z - znew).topRows<2>(); 121 | 122 | Vector3d v = Vector3d::Zero(); 123 | Vector2d res = Vector2d::Zero(); 124 | 125 | prange_factor(x.data(), v.data(), clk_bias.data(), x_e2n.data(), res.data()); 126 | 127 | EXPECT_MAT_NEAR(true_res, res, 1e-4); 128 | } 129 | 130 | 131 | TEST (Pseudorange, PointPositioning) 132 | { 133 | GTime rec_time{2026, 165029.0}; 134 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 135 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 136 | Vector3d rec_vel_NED{1, 2, 3}; 137 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 138 | Vector3d rec_vel_ECEF = x_e2n.q().rota(rec_vel_NED); 139 | 140 | std::vector sats; 141 | for (int i = 0; i < 100; i++) 142 | { 143 | Satellite sat(i, sats.size()); 144 | sat.readFromRawFile("../lib/multirotor_sim/sample/eph.dat"); 145 | if (sat.eph_.A > 0) 146 | { 147 | sats.push_back(sat); 148 | } 149 | } 150 | 151 | ceres::Problem problem; 152 | Xformd xhat = Xformd::Identity(); 153 | xhat.t().x() -= 1000; 154 | Vector3d vhat = Vector3d::Zero(); 155 | Vector2d clk_bias_hat{0.0, 0.0}; 156 | problem.AddParameterBlock(xhat.data(), 7, new XformParamAD); 157 | problem.AddParameterBlock(x_e2n.data(), 7, new XformParamAD); 158 | problem.AddParameterBlock(vhat.data(), 3); 159 | problem.AddParameterBlock(clk_bias_hat.data(), 2); 160 | problem.SetParameterBlockConstant(x_e2n.data()); 161 | 162 | std::vector::iterator sat; 163 | for (sat = sats.begin(); sat != sats.end(); sat++) 164 | { 165 | Vector3d meas; 166 | Matrix2d cov = Vector2d{0.1, 0.1}.asDiagonal(); 167 | sat->computeMeasurement(rec_time, rec_pos, rec_vel_ECEF, Vector2d::Zero(), meas); 168 | problem.AddResidualBlock(new PRangeFactorAD( 169 | new PRangeFunctor(rec_time, meas.topRows<2>(), *sat, rec_pos, cov)), 170 | NULL, xhat.data(), vhat.data(), clk_bias_hat.data(), x_e2n.data()); 171 | } 172 | 173 | 174 | ceres::Solver::Options options; 175 | options.max_num_iterations = 100; 176 | options.num_threads = 6; 177 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 178 | options.minimizer_progress_to_stdout = false; 179 | ceres::Solver::Summary summary; 180 | 181 | // cout << "xhat0\n" << xhat << endl; 182 | // cout << "vhat0\n" << vhat.transpose() << endl; 183 | // cout << "dthat0\n" << clk_bias_hat.transpose() << endl; 184 | 185 | ceres::Solve(options, &problem, &summary); 186 | 187 | // cout << "xhatf\n" << xhat << endl; 188 | // cout << "vhatf\n" << vhat.transpose() << endl; 189 | // cout << "dthatf\n" << clk_bias_hat.transpose() << endl; 190 | 191 | double xerror = xhat.t().norm(); 192 | double verror = (vhat - rec_vel_NED).norm(); 193 | double dterror = (clk_bias_hat).norm(); 194 | 195 | // cout << "xerror: " << xerror << endl; 196 | // cout << "verror: " << verror << endl; 197 | // cout << "dterror: " << dterror << endl; 198 | 199 | EXPECT_NEAR(xerror, 0.0, 1e-2); 200 | EXPECT_NEAR(verror, 0.0, 1e-2); ///TODO: Figure out why this is so large 201 | EXPECT_NEAR(dterror, 0.0, 1e-8); 202 | } 203 | 204 | TEST (Pseudorange, Trajectory) 205 | { 206 | TrajectorySim a(raw_gps_yaml_file()); 207 | 208 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 209 | a.addNodeCB([&a](){a.addPseudorangeFactors();}); 210 | 211 | a.run(); 212 | a.solve(); 213 | a.log("/tmp/ceres_sandbox/Pseudorange.Trajectory.log"); 214 | 215 | // This one actually doesn't work, because you need to estimate clock dynamics 216 | // it is informative of whether the clock dynamics estimation is working though 217 | // ASSERT_LE(a.final_error(), a.error0); 218 | } 219 | 220 | TEST (Pseudorange, TrajectoryClockDynamics) 221 | { 222 | TrajectorySim a(raw_gps_yaml_file()); 223 | 224 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 225 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 226 | 227 | a.run(); 228 | a.solve(); 229 | a.log("/tmp/ceres_sandbox/Pseudorange.TrajectoryClockDynamics.log"); 230 | 231 | // ASSERT_LE(a.final_error(), a.error0); 232 | } 233 | 234 | TEST (Pseudorange, ImuTrajectory) 235 | { 236 | TrajectorySim a(raw_gps_yaml_file()); 237 | 238 | a.addNodeCB([&a](){a.addImuFactor();}); 239 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 240 | a.addNodeCB([&a](){a.addPseudorangeFactors();}); 241 | 242 | a.run(); 243 | a.solve(); 244 | a.log("/tmp/ceres_sandbox/Pseudorange.ImuTrajectory.log"); 245 | 246 | // ASSERT_LE(a.final_error(), a.error0); 247 | } 248 | 249 | TEST (Pseudorange, ImuTrajectoryClockDynamics) 250 | { 251 | TrajectorySim a(raw_gps_yaml_file()); 252 | 253 | a.addNodeCB([&a](){a.addImuFactor();}); 254 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 255 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 256 | 257 | a.run(); 258 | a.solve(); 259 | a.log("/tmp/ceres_sandbox/Pseudorange.ImuTrajectoryClockDynamics.log"); 260 | 261 | ASSERT_LE(a.final_error(), a.error0); 262 | } 263 | 264 | TEST (MultipathPseudorange, StandardResidual) 265 | { 266 | TrajectorySim a(raw_gps_multipath_yaml_file()); 267 | 268 | a.addNodeCB([&a](){a.addImuFactor();}); 269 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 270 | a.addNodeCB([&a](){a.addPseudorangeFactorsWithClockDynamics();}); 271 | 272 | a.run(); 273 | a.solve(); 274 | a.log("/tmp/ceres_sandbox/MultipathPseudorange.StandardResidual.log"); 275 | 276 | ASSERT_LE(a.final_error(), a.error0); 277 | } 278 | 279 | TEST (MultipathPseudorange, SwitchingResidual) 280 | { 281 | TrajectorySim a(raw_gps_multipath_yaml_file()); 282 | 283 | a.addNodeCB([&a](){a.addImuFactor();}); 284 | a.addNodeCB([&a](){a.initNodePostionFromPointPos();}); 285 | a.addNodeCB([&a](){a.addSwitchingPseudorangeFactors();}); 286 | 287 | a.run(); 288 | a.solve(); 289 | a.log("/tmp/ceres_sandbox/MultipathPseudorange.SwitchingResidual.log"); 290 | 291 | ASSERT_LE(a.final_error(), a.error0); 292 | } 293 | 294 | 295 | 296 | -------------------------------------------------------------------------------- /src/switch.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "geometry/xform.h" 10 | #include "multirotor_sim/simulator.h" 11 | #include "multirotor_sim/controller.h" 12 | #include "multirotor_sim/satellite.h" 13 | #include "multirotor_sim/estimator_wrapper.h" 14 | #include "utils/logger.h" 15 | #include "test_common.h" 16 | 17 | #include "factors/switch_pseudorange.h" 18 | #include "factors/pseudorange.h" 19 | 20 | 21 | using namespace multirotor_sim; 22 | using namespace Eigen; 23 | using namespace xform; 24 | 25 | class TestSwitchPseudorange 26 | { 27 | public: 28 | TestSwitchPseudorange() : 29 | sat(1, 0) 30 | { 31 | init_sat(); 32 | init_func(); 33 | } 34 | 35 | void init_sat() 36 | { 37 | time.week = 86400.00 / DateTime::SECONDS_IN_WEEK; 38 | time.tow_sec = 86400.00 - (time.week * DateTime::SECONDS_IN_WEEK); 39 | 40 | eph.sat = 1; 41 | eph.A = 5153.79589081 * 5153.79589081; 42 | eph.toe.week = 93600.0 / DateTime::SECONDS_IN_WEEK; 43 | eph.toe.tow_sec = 93600.0 - (eph.toe.week * DateTime::SECONDS_IN_WEEK); 44 | eph.toes = 93600.0; 45 | eph.deln = 0.465376527657e-08; 46 | eph.M0 = 1.05827953357; 47 | eph.e = 0.00223578442819; 48 | eph.omg = 2.06374037770; 49 | eph.cus = 0.177137553692e-05; 50 | eph.cuc = 0.457651913166e-05; 51 | eph.crs = 88.6875000000; 52 | eph.crc = 344.96875; 53 | eph.cis = -0.856816768646e-07; 54 | eph.cic = 0.651925802231e-07; 55 | eph.idot = 0.342514267094e-09; 56 | eph.i0 = 0.961685061380; 57 | eph.OMG0 = 1.64046615454; 58 | eph.OMGd = -0.856928551657e-08; 59 | sat.addEphemeris(eph); 60 | } 61 | 62 | void init_func() 63 | { 64 | Vector2d rho; 65 | sat.computeMeasurement(time, rec_pos, Vector3d::Zero(), clk_bias, z); 66 | rho = z.topRows<2>(); 67 | Matrix2d cov = (Vector2d{3.0, 0.4}).asDiagonal(); 68 | 69 | f = new SwitchPRangeFunctor(time, rho, sat, rec_pos, cov, 1.0, 1.0); 70 | 71 | x.t() << 10, 0, 0; 72 | Vector3d p_ecef = WSG84::ned2ecef(x_e2n, x.t()); 73 | sat.computeMeasurement(time, p_ecef, Vector3d::Zero(), clk_bias, znew); 74 | true_res << Vector2d{std::sqrt(1/3.0), std::sqrt(1/0.4)}.asDiagonal() * (z - znew).topRows<2>(), 0; 75 | } 76 | 77 | Vector3d provo_lla{40.246184 * DEG2RAD , -111.647769 * DEG2RAD, 1387.997511}; 78 | Vector3d rec_pos = WSG84::lla2ecef(provo_lla); 79 | Xformd x_e2n = WSG84::x_ecef2ned(rec_pos); 80 | Vector2d clk_bias{1e-8, 1e-6}; 81 | Vector3d v = Vector3d::Zero(); 82 | Vector3d res = Vector3d::Zero(); 83 | Xformd x = Xformd::Identity(); 84 | Vector3d z; 85 | Vector3d znew; 86 | Vector3d true_res; 87 | 88 | SwitchPRangeFunctor* f; 89 | eph_t eph; 90 | GTime time; 91 | Satellite sat; 92 | }; 93 | 94 | TEST (SwitchPRangeFunctor, SwitchOn) 95 | { 96 | TestSwitchPseudorange a; 97 | double s = 1.0; 98 | Vector3d res; 99 | (*a.f)(a.x.data(), a.v.data(), a.clk_bias.data(), a.x_e2n.data(), &s, res.data()); 100 | 101 | EXPECT_MAT_NEAR(a.true_res, res, 1e-4); 102 | } 103 | 104 | TEST (SwitchPRangeFunctor, SwitchOff) 105 | { 106 | TestSwitchPseudorange a; 107 | double s = 0.0; 108 | Vector3d res; 109 | (*a.f)(a.x.data(), a.v.data(), a.clk_bias.data(), a.x_e2n.data(), &s, res.data()); 110 | 111 | EXPECT_MAT_NEAR(Vector3d(0, 0, 1.0), res, 1e-4); 112 | } 113 | 114 | TEST (SwitchPRangeFunctor, SwitchAboveOne) 115 | { 116 | TestSwitchPseudorange a; 117 | double s = 4.0; 118 | Vector3d res; 119 | (*a.f)(a.x.data(), a.v.data(), a.clk_bias.data(), a.x_e2n.data(), &s, res.data()); 120 | 121 | EXPECT_MAT_NEAR(a.true_res, res, 1e-4); 122 | } 123 | 124 | TEST (SwitchPRangeFunctor, SwitchBelowZero) 125 | { 126 | TestSwitchPseudorange a; 127 | double s = -4.0; 128 | Vector3d res; 129 | (*a.f)(a.x.data(), a.v.data(), a.clk_bias.data(), a.x_e2n.data(), &s, res.data()); 130 | 131 | EXPECT_MAT_NEAR(Vector3d(0, 0, 1.0), res, 1e-4); 132 | } 133 | 134 | TEST (SwitchPRangeFunctor, TrajectoryMultipath) 135 | { 136 | 137 | } 138 | 139 | 140 | -------------------------------------------------------------------------------- /src/test_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "test_common.h" 4 | 5 | using namespace std; 6 | 7 | std::string imu_only_sim() 8 | { 9 | YAML::Node yaml; 10 | yaml = YAML::LoadFile("../lib/multirotor_sim/params/sim_params.yaml"); 11 | ofstream tmp("/tmp/ceres_sandbox.tmp.yaml"); 12 | yaml["imu_enabled"] = true; 13 | yaml["alt_enabled"] = false; 14 | yaml["mocap_enabled"] = false; 15 | yaml["vo_enabled"] = false; 16 | yaml["camera_enabled"] = false; 17 | yaml["gnss_enabled"] = false; 18 | yaml["raw_gnss_enabled"] = false; 19 | tmp << yaml; 20 | tmp.close(); 21 | return "/tmp/ceres_sandbox.tmp.yaml"; 22 | } 23 | 24 | std::string raw_gps_yaml_file() 25 | { 26 | YAML::Node node; 27 | node = YAML::LoadFile("../lib/multirotor_sim/params/sim_params.yaml"); 28 | ofstream tmp("/tmp/ceres_sandbox.tmp.yaml"); 29 | node["imu_enabled"] = true; 30 | node["alt_enabled"] = false; 31 | node["mocap_enabled"] = false; 32 | node["vo_enabled"] = false; 33 | node["camera_enabled"] = false; 34 | node["gnss_enabled"] = false; 35 | node["raw_gnss_enabled"] = true; 36 | node["ephemeris_filename"] = "../lib/multirotor_sim/sample/eph.dat"; 37 | 38 | tmp << node; 39 | tmp.close(); 40 | return "/tmp/ceres_sandbox.tmp.yaml"; 41 | } 42 | 43 | std::string raw_gps_multipath_yaml_file() 44 | { 45 | YAML::Node node; 46 | node = YAML::LoadFile("../lib/multirotor_sim/params/sim_params.yaml"); 47 | ofstream tmp("/tmp/ceres_sandbox.tmp.yaml"); 48 | node["imu_enabled"] = true; 49 | node["alt_enabled"] = false; 50 | node["mocap_enabled"] = false; 51 | node["vo_enabled"] = false; 52 | node["camera_enabled"] = false; 53 | node["gnss_enabled"] = false; 54 | node["raw_gnss_enabled"] = true; 55 | node["ephemeris_filename"] = "../lib/multirotor_sim/sample/eph.dat"; 56 | 57 | node["multipath_prob"] = 0.1; 58 | node["cycle_slip_prob"] = 0.0; 59 | 60 | tmp << node; 61 | tmp.close(); 62 | return "/tmp/ceres_sandbox.tmp.yaml"; 63 | } 64 | 65 | std::string mocap_yaml_file() 66 | { 67 | YAML::Node node; 68 | node = YAML::LoadFile("../lib/multirotor_sim/params/sim_params.yaml"); 69 | ofstream tmp("/tmp/ceres_sandbox.tmp.yaml"); 70 | node["imu_enabled"] = true; 71 | node["alt_enabled"] = false; 72 | node["mocap_enabled"] = true; 73 | node["vo_enabled"] = false; 74 | node["camera_enabled"] = false; 75 | node["gnss_enabled"] = false; 76 | node["raw_gnss_enabled"] = false; 77 | 78 | tmp << node; 79 | tmp.close(); 80 | return "/tmp/ceres_sandbox.tmp.yaml"; 81 | } 82 | 83 | -------------------------------------------------------------------------------- /src/time_offset.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "gtest/gtest.h" 8 | 9 | #include "factors/range_1d.h" 10 | #include "factors/position_1d.h" 11 | #include "factors/imu_1d.h" 12 | #include "factors/SE3.h" 13 | #include "factors/imu_3d.h" 14 | 15 | #include "utils/robot1d.h" 16 | #include "utils/jac.h" 17 | #include "utils/logger.h" 18 | 19 | #include "multirotor_sim/estimator_wrapper.h" 20 | #include "multirotor_sim/simulator.h" 21 | #include "multirotor_sim/controller.h" 22 | #include "test_common.h" 23 | 24 | 25 | using namespace ceres; 26 | using namespace Eigen; 27 | using namespace std; 28 | using namespace multirotor_sim; 29 | 30 | TEST(TimeOffset, 1DRobotSLAM) 31 | { 32 | double ba = 0.2; 33 | double bahat = 0.00; 34 | 35 | double Td = 0.05; 36 | double Tdhat = 0.0; 37 | 38 | double Q = 1e-3; 39 | 40 | Robot1D Robot(ba, Q, Td); 41 | Robot.waypoints_ = {3, 0, 3, 0}; 42 | 43 | const int num_windows = 15; 44 | int window_size = 50; 45 | double dt = 0.01; 46 | 47 | Eigen::Matrix landmarks = (Eigen::Matrix() << -20, -15, -10, -5, 5, 10, 15, 20).finished(); 48 | Eigen::Matrix lhat = (Eigen::Matrix() << -21, -13, -8, -5, 3, 12, 16, 23).finished(); 49 | 50 | double rvar = 1e-2; 51 | std::default_random_engine gen; 52 | std::normal_distribution normal(0.0,1.0); 53 | 54 | Problem problem; 55 | 56 | Eigen::Matrix xhat; 57 | Eigen::Matrix x; 58 | 59 | 60 | // Initialize the Graph 61 | xhat(0,0) = Robot.xhat_; 62 | xhat(1,0) = Robot.vhat_; 63 | x(0,0) = Robot.x_; 64 | x(1,0) = Robot.v_; 65 | 66 | // Tie the graph to the origin 67 | problem.AddParameterBlock(xhat.data(), 2); 68 | problem.SetParameterBlockConstant(xhat.data()); 69 | 70 | for (int win = 1; win < num_windows; win++) 71 | { 72 | Imu1DFunctor *IMUFactor = new Imu1DFunctor(Robot.t_, bahat, Q); 73 | while (Robot.t_ < win*window_size*dt) 74 | { 75 | Robot.step(dt); 76 | IMUFactor->integrate(Robot.t_, Robot.ahat_); 77 | } 78 | 79 | // Save the actual state 80 | x(0, win) = Robot.x_; 81 | x(1, win) = Robot.v_; 82 | 83 | // Guess at next state 84 | xhat.col(win) = IMUFactor->estimate_xj(xhat.col(win-1)); 85 | IMUFactor->finished(); // Calculate the Information matrix in preparation for optimization 86 | 87 | // Add preintegrated IMU 88 | problem.AddParameterBlock(xhat.data()+2*win, 2); 89 | problem.AddResidualBlock(new Imu1DFactorAD(IMUFactor), NULL, xhat.data()+2*(win-1), xhat.data()+2*win, &bahat); 90 | 91 | // Add landmark measurements 92 | for (int l = 0; l < landmarks.size(); l++) 93 | { 94 | double rbar = (landmarks[l] - Robot.x_) + normal(gen)*std::sqrt(rvar); 95 | problem.AddResidualBlock(new RangeVel1DFactor(rbar, rvar), NULL, lhat.data()+l, xhat.data()+2*win); 96 | } 97 | 98 | // Add lagged position measurement 99 | double xbar_lag = Robot.hist_.front().x; 100 | double xbar_lag_cov = 1e-8; 101 | problem.AddResidualBlock(new Pos1DTimeOffsetFactor(xbar_lag, xbar_lag_cov), NULL, xhat.data()+2*win, &Tdhat); 102 | } 103 | 104 | Solver::Options options; 105 | options.max_num_iterations = 100; 106 | options.linear_solver_type = ceres::DENSE_QR; 107 | options.minimizer_progress_to_stdout = false; 108 | 109 | Solver::Summary summary; 110 | 111 | // cout << "xhat0: \n" << xhat << endl; 112 | // cout << "lhat0: \n" << lhat.transpose() << endl; 113 | // cout << "bahat0 : " << bahat << endl; 114 | // cout << "Tdhat0 : " << bahat << endl; 115 | // cout << "e0 : " << (x - xhat).array().abs().sum() << endl; 116 | ceres::Solve(options, &problem, &summary); 117 | // cout << "x: \n" << x <(0,0) = multirotor.accel_bias_; 139 | b.block<3,1>(3,0) = multirotor.gyro_bias_; 140 | bhat.setZero(); 141 | 142 | double dt, dthat; 143 | dt = 0.01; 144 | dthat = 0.0; 145 | 146 | multirotor.mocap_transmission_time_ = dt; 147 | multirotor.mocap_update_rate_ = 10; 148 | 149 | 150 | Eigen::MatrixXd xhat, x; 151 | Eigen::MatrixXd vhat, v; 152 | xhat.resize(7, N+1); 153 | x.resize(7, N+1); 154 | vhat.resize(3, N+1); 155 | v.resize(3, N+1); 156 | xhat.setZero(); 157 | xhat.row(3).setConstant(1.0); 158 | vhat.setZero(); 159 | 160 | Problem problem; 161 | problem.AddParameterBlock(&dthat, 1); 162 | problem.AddParameterBlock(bhat.data(), 6); 163 | for (int n = 0; n < N; n++) 164 | { 165 | problem.AddParameterBlock(xhat.data()+7*n, 7, new XformParamAD()); 166 | problem.AddParameterBlock(vhat.data()+3*n, 3); 167 | } 168 | 169 | xhat.col(0) = multirotor.state().X.arr_; 170 | vhat.col(0) = multirotor.state().v; 171 | x.col(0) = multirotor.state().X.arr_; 172 | v.col(0) = multirotor.state().v; 173 | 174 | std::vector factors; 175 | Matrix6d cov = multirotor.imu_R_; 176 | factors.push_back(new Imu3DFunctor(0, bhat)); 177 | 178 | // Integrate for N frames 179 | int node = 0; 180 | Imu3DFunctor* factor = factors[node]; 181 | std::vector t; 182 | t.push_back(multirotor.t_); 183 | Xformd prev2_pose, prev_pose, current_pose; 184 | double prev2_t, prev_t, current_t; 185 | Matrix6d pose_cov = Matrix6d::Constant(0); 186 | 187 | bool new_node; 188 | auto imu_cb = [&factor](const double& t, const Vector6d& z, const Matrix6d& R) 189 | { 190 | factor->integrate(t, z, R); 191 | }; 192 | auto mocap_cb = [&pose_cov, &new_node, ¤t_pose](const double& t, const Xformd& z, const Matrix6d& R) 193 | { 194 | (void)t; 195 | new_node = true; 196 | pose_cov = R; 197 | current_pose = z; 198 | }; 199 | EstimatorWrapper est; 200 | est.register_imu_cb(imu_cb); 201 | est.register_mocap_cb(mocap_cb); 202 | multirotor.register_estimator(&est); 203 | 204 | while (node < N) 205 | { 206 | new_node = false; 207 | multirotor.run(); 208 | current_t = multirotor.t_; 209 | 210 | if (new_node) 211 | { 212 | t.push_back(multirotor.t_); 213 | node += 1; 214 | 215 | // estimate next node pose and velocity with IMU preintegration 216 | factor->estimateXj(xhat.data()+7*(node-1), vhat.data()+3*(node-1), 217 | xhat.data()+7*(node), vhat.data()+3*(node)); 218 | // Calculate the Information Matrix of the IMU factor 219 | factor->finished(); 220 | 221 | xhat.col(node) = current_pose.arr_; 222 | 223 | // Save off True Pose and Velocity for Comparison 224 | x.col(node) = multirotor.state().X.arr_; 225 | v.col(node) = multirotor.state().v; 226 | 227 | // Add IMU factor to graph 228 | problem.AddResidualBlock(new Imu3DFactorAD(factor), 229 | NULL, xhat.data()+7*(node-1), xhat.data()+7*node, vhat.data()+3*(node-1), vhat.data()+3*node, bhat.data()); 230 | 231 | // Start a new Factor 232 | factors.push_back(new Imu3DFunctor(multirotor.t_, bhat)); 233 | factor = factors[node]; 234 | 235 | Matrix6d P = Matrix6d::Identity() * 0.001; 236 | Vector6d current_pose_dot; 237 | current_pose_dot.segment<3>(0) = v.col(node); 238 | current_pose_dot.segment<3>(3) = multirotor.state().w; 239 | problem.AddResidualBlock(new XformTimeOffsetAD( 240 | new XformTimeOffsetFunctor(x.col(node), current_pose_dot, pose_cov)), NULL, xhat.data()+7*node, &dthat); 241 | } 242 | } 243 | 244 | Solver::Options options; 245 | options.max_num_iterations = 100; 246 | options.num_threads = 6; 247 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 248 | options.minimizer_progress_to_stdout = false; 249 | Solver::Summary summary; 250 | Logger log("/tmp/ceres_sandbox/TimeOffset.MultiWindowConstantBias.log"); 251 | 252 | MatrixXd xhat0 = xhat; 253 | MatrixXd vhat0 = vhat; 254 | // cout.flush(); 255 | 256 | // cout << "xhat0\n" << xhat.transpose() << endl; 257 | // cout << "bhat0\n" << bhat.transpose() << endl; 258 | // cout << "dthat0: " << dthat << endl; 259 | 260 | ceres::Solve(options, &problem, &summary); 261 | double error = (b - bhat).norm(); 262 | 263 | // cout << summary.FullReport(); 264 | // cout << "x\n" << x.transpose() << endl; 265 | // cout << "xhatf\n" << xhat.transpose() << endl; 266 | // cout << "b: " << b.transpose() << endl; 267 | // cout << "bhat: " << bhat.transpose() << endl; 268 | // cout << "err: " << (b - bhat).transpose() << endl; 269 | 270 | // cout << "dt: " << dt << endl; 271 | // cout << "dthatf: " << dthat << endl; 272 | // cout << "e " << error << endl; 273 | EXPECT_LE(error, 0.2); 274 | EXPECT_NEAR(dt, -dthat, 0.01); 275 | 276 | // Eigen::Matrix final_residuals; 277 | 278 | // cout << "R\n"; 279 | // for (int node = 1; node <= N; node++) 280 | // { 281 | // (*factors[node-1])(xhat.data()+7*(node-1), xhat.data()+7*node, 282 | // vhat.data()+3*(node-1), vhat.data()+3*node, 283 | // bhat.data(), 284 | // final_residuals.data()+9*node); 285 | // cout << final_residuals.col(node-1).transpose() << "\n"; 286 | 287 | // } 288 | // cout << endl; 289 | 290 | 291 | 292 | for (int i = 0; i <= N; i++) 293 | { 294 | log.log(t[i]); 295 | log.logVectors(xhat0.col(i), vhat0.col(i), xhat.col(i), vhat.col(i), x.col(i), v.col(i)); 296 | } 297 | } 298 | 299 | --------------------------------------------------------------------------------