├── .gitignore
├── CMakeLists.txt
├── baproblem.cpp
├── ba_demo.cpp
├── ba_demo_6d.cpp
├── LICENSE
├── README.md
├── .vscode
    └── c_cpp_properties.json
├── parametersse3.hpp
├── baproblem.hpp
├── parametersse3.cpp
└── se3.hpp


/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 | *.user
3 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | project(ba_demo_ceres)
 4 | 
 5 | find_package(Ceres REQUIRED)
 6 | 
 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 ")
 8 | 
 9 | include_directories(${CERES_INCLUDE_DIRS} )
10 | 
11 | add_executable(${PROJECT_NAME} ba_demo.cpp baproblem.cpp parametersse3.cpp)
12 | target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES} )
13 | 
14 | add_executable(ba_6d ba_demo_6d.cpp baproblem.cpp parametersse3.cpp)
15 | target_link_libraries(ba_6d ${CERES_LIBRARIES})
16 | 


--------------------------------------------------------------------------------
/baproblem.cpp:
--------------------------------------------------------------------------------
 1 | #include "baproblem.hpp"
 2 | #include <cstdlib>
 3 | 
 4 | using namespace Eigen;
 5 | using namespace std;
 6 | 
 7 | static double uniform_rand(double lowerBndr, double upperBndr)
 8 | {
 9 |     return lowerBndr + ((double)std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
10 | }
11 | 
12 | static double gauss_rand(double mean, double sigma)
13 | {
14 |     double x, y, r2;
15 |     do
16 |     {
17 |         x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
18 |         y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
19 |         r2 = x * x + y * y;
20 |     } while (r2 > 1.0 || r2 == 0.0);
21 |     return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
22 | }
23 | 
24 | int Sample::uniform(int from, int to)
25 | {
26 |     return static_cast<int>(uniform_rand(from, to));
27 | }
28 | 
29 | double Sample::uniform()
30 | {
31 |     return uniform_rand(0., 1.);
32 | }
33 | 
34 | double Sample::gaussian(double sigma)
35 | {
36 |     return gauss_rand(0., sigma);
37 | }
38 | 
39 | 
40 | 


--------------------------------------------------------------------------------
/ba_demo.cpp:
--------------------------------------------------------------------------------
 1 | #include "baproblem.hpp"
 2 | 
 3 | using namespace Eigen;
 4 | using namespace std;
 5 | 
 6 | constexpr int USE_POSE_SIZE = 7;
 7 | 
 8 | int main(int argc, const char *argv[])
 9 | {
10 |     if (argc < 2)
11 |     {
12 |         cout << endl;
13 |         cout << "Please type: " << endl;
14 |         cout << "ba_demo [PIXEL_NOISE] " << endl;
15 |         cout << endl;
16 |         cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
17 |         cout << endl;
18 |         exit(0);
19 |     }
20 | 
21 |     google::InitGoogleLogging(argv[0]);
22 | 
23 |     double PIXEL_NOISE = atof(argv[1]);
24 | 
25 |     cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
26 | 
27 |     BAProblem<USE_POSE_SIZE> baProblem(15, 300, PIXEL_NOISE, true);
28 | 
29 |     ceres::Solver::Options options;
30 |     options.linear_solver_type = ceres::SPARSE_SCHUR;
31 |     options.minimizer_progress_to_stdout = true;
32 |     options.max_num_iterations = 50;
33 |     ceres::Solver::Summary summary;
34 |     baProblem.solve(options, &summary);
35 |     std::cout << summary.BriefReport() << "\n";
36 | }
37 | 


--------------------------------------------------------------------------------
/ba_demo_6d.cpp:
--------------------------------------------------------------------------------
 1 | #include "baproblem.hpp"
 2 | 
 3 | using namespace Eigen;
 4 | using namespace std;
 5 | 
 6 | constexpr int USE_POSE_SIZE = 6;
 7 | 
 8 | int main(int argc, const char *argv[])
 9 | {
10 |     if (argc < 2)
11 |     {
12 |         cout << endl;
13 |         cout << "Please type: " << endl;
14 |         cout << "ba_demo [PIXEL_NOISE] " << endl;
15 |         cout << endl;
16 |         cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
17 |         cout << endl;
18 |         exit(0);
19 |     }
20 | 
21 |     google::InitGoogleLogging(argv[0]);
22 | 
23 |     double PIXEL_NOISE = atof(argv[1]);
24 | 
25 |     cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
26 | 
27 |     BAProblem<USE_POSE_SIZE> baProblem(15, 300, PIXEL_NOISE, true);
28 | 
29 |     ceres::Solver::Options options;
30 |     options.linear_solver_type = ceres::SPARSE_SCHUR;
31 |     options.minimizer_progress_to_stdout = true;
32 |     options.max_num_iterations = 50;
33 |     ceres::Solver::Summary summary;
34 |     baProblem.solve(options, &summary);
35 |     std::cout << summary.BriefReport() << "\n";
36 | }
37 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2017, ZHENG Fan 郑帆
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 | * Redistributions of source code must retain the above copyright notice, this
10 |   list of conditions and the following disclaimer.
11 | 
12 | * Redistributions in binary form must reproduce the above copyright notice,
13 |   this list of conditions and the following disclaimer in the documentation
14 |   and/or other materials provided with the distribution.
15 | 
16 | * Neither the name of the copyright holder nor the names of its
17 |   contributors may be used to endorse or promote products derived from
18 |   this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ba_demo_ceres
 2 | 
 3 | Bundle adjustment demo using Ceres Solver
 4 | 
 5 | Also read a [blog post](http://fzheng.me/2018/01/23/ba-demo-ceres/).
 6 | 
 7 | ### Dependencies
 8 | 
 9 | - Ceres Solver
10 | 
11 | ### Introduction 
12 | 
13 | This project implements a simple bundle adjustment problem using Cere Solver to solve it. 
14 | 
15 | The constraints are defined with customized cost functions and self-defined Jacobians.
16 | 
17 | Two versions of on-manifold local parameterization of SE3 poses (both of which using 6-dof Lie algebra for updating) are used, including
18 | 
19 |   - quaternion + translation vector (7d), and
20 |   - rotation vector + translation vector (6d)
21 | 
22 | Please check `parametersse3.hpp` `parametersse3.cpp` for the local parameterization implementation.
23 | 
24 | ### Jacobians
25 | 
26 | For convenience in expression, we denote the Jacobian matrix of the error function w.r.t. the Lie group by `J_err_grp`, the Jacobian matrix of the Lie group w.r.t. the local Lie algebra increment by `J_grp_alg`, and the Jacobian matrix of the error function w.r.t. the local Lie algebra increment by `J_err_alg`.
27 | 
28 | In many on-manifold graph optimization problems, only `J_err_alg` are required.  However, in Ceres Solver, one can only seperately assign `J_err_grp` and `J_grp_alg`, but cannot directly define `J_err_alg`. This may be redundant and cost extra computational resources:
29 | 
30 |   - One has to derive the explicit Jacobians equations of both `J_err_grp` and `J_grp_alg`.
31 |   - The solver may spend extra time in computing `J_err_grp` and `J_grp_alg`, and multiplying `J_err_grp` and `J_grp_alg` to get `J_err_alg`.
32 | 
33 | Therefore, for convenience, we use a not-that-elegant but effective trick. Let's say that the error function term is of dimension `m`, the Lie group `N`, and the Lie algebra `n`. We define the leading `m*n` block in  `J_err_grp` to be the actual `J_err_alg`, with other elements to be zero; and define the leading `n*n` block in `J_grp_alg` to be identity matrix, with other elements to be zero. Thus, we are free of deriving the two extra Jacobians, and the computational burden of the solver is reduced -- although Ceres still has to multiply the two Jacobians, the overall computational process gets simpler.
34 |  
35 | ### Other tips
36 | 
37 | One advice for self-defined Jacobians: do not access jacobians[0] and jacobians[1] in the meantime, which may cause seg-fault.
38 | 
39 | 
40 | ### License 
41 | 
42 | [BSD New](LICENSE)
43 | 


--------------------------------------------------------------------------------
/.vscode/c_cpp_properties.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "configurations": [
 3 |         {
 4 |             "name": "Mac",
 5 |             "includePath": [
 6 |                 "/usr/include",
 7 |                 "/usr/local/include",
 8 |                 "${workspaceRoot}"
 9 |             ],
10 |             "defines": [],
11 |             "intelliSenseMode": "clang-x64",
12 |             "browse": {
13 |                 "path": [
14 |                     "/usr/include",
15 |                     "/usr/local/include",
16 |                     "${workspaceRoot}"
17 |                 ],
18 |                 "limitSymbolsToIncludedHeaders": true,
19 |                 "databaseFilename": ""
20 |             },
21 |             "macFrameworkPath": [
22 |                 "/System/Library/Frameworks",
23 |                 "/Library/Frameworks"
24 |             ]
25 |         },
26 |         {
27 |             "name": "Linux",
28 |             "includePath": [
29 |                 "/usr/include/c++/5",
30 |                 "/usr/include/x86_64-linux-gnu/c++/5",
31 |                 "/usr/include/c++/5/backward",
32 |                 "/usr/lib/gcc/x86_64-linux-gnu/5/include",
33 |                 "/usr/local/include",
34 |                 "/usr/lib/gcc/x86_64-linux-gnu/5/include-fixed",
35 |                 "/usr/include/x86_64-linux-gnu",
36 |                 "/usr/include",
37 |                 "/usr/include/eigen3",
38 |                 "/usr/include/suitesparse",
39 |                 "${workspaceRoot}"
40 |             ],
41 |             "defines": [],
42 |             "intelliSenseMode": "clang-x64",
43 |             "browse": {
44 |                 "path": [
45 |                     "/usr/include/c++/5",
46 |                     "/usr/include/x86_64-linux-gnu/c++/5",
47 |                     "/usr/include/c++/5/backward",
48 |                     "/usr/lib/gcc/x86_64-linux-gnu/5/include",
49 |                     "/usr/local/include",
50 |                     "/usr/lib/gcc/x86_64-linux-gnu/5/include-fixed",
51 |                     "/usr/include/x86_64-linux-gnu",
52 |                     "/usr/include",
53 |                     "${workspaceRoot}"
54 |                 ],
55 |                 "limitSymbolsToIncludedHeaders": true,
56 |                 "databaseFilename": ""
57 |             }
58 |         },
59 |         {
60 |             "name": "Win32",
61 |             "includePath": [
62 |                 "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include",
63 |                 "${workspaceRoot}"
64 |             ],
65 |             "defines": [
66 |                 "_DEBUG",
67 |                 "UNICODE"
68 |             ],
69 |             "intelliSenseMode": "msvc-x64",
70 |             "browse": {
71 |                 "path": [
72 |                     "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include/*",
73 |                     "${workspaceRoot}"
74 |                 ],
75 |                 "limitSymbolsToIncludedHeaders": true,
76 |                 "databaseFilename": ""
77 |             }
78 |         }
79 |     ],
80 |     "version": 3
81 | }


--------------------------------------------------------------------------------
/parametersse3.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef PARAMETERSSE3_HPP
  2 | #define PARAMETERSSE3_HPP
  3 | 
  4 | #include <Eigen/StdVector>
  5 | #include <Eigen/Geometry>
  6 | #include <iostream>
  7 | 
  8 | #include <ceres/ceres.h>
  9 | 
 10 | #include "se3.hpp"
 11 | 
 12 | using namespace std;
 13 | using namespace Eigen;
 14 | 
 15 | class CameraParameters
 16 | {
 17 | protected:
 18 |     double f;
 19 |     double cx;
 20 |     double cy;
 21 | public:
 22 |     CameraParameters(double f_, double cx_, double cy_)
 23 |         : f(f_), cx(cx_), cy(cy_) {}
 24 | 
 25 |     Vector2d cam_map(const Vector3d& p)
 26 |     {
 27 |         Vector2d z;
 28 |         z[0] = f * p[0] / p[2] + cx;
 29 |         z[1] = f * p[1] / p[2] + cy;
 30 |         return z;
 31 |     }
 32 | };
 33 | 
 34 | /// PoseBlockSize can only be
 35 | /// 7 (quaternion + translation vector) or
 36 | /// 6 (rotation vector + translation vector)
 37 | template<int PoseBlockSize>
 38 | class ReprojectionErrorSE3XYZ: public ceres::SizedCostFunction<2, PoseBlockSize, 3>
 39 | {
 40 | public:
 41 |     ReprojectionErrorSE3XYZ(double f_,
 42 |                             double cx_,
 43 |                             double cy_,
 44 |                             double observation_x,
 45 |                             double observation_y)
 46 |         : f(f_), cx(cx_), cy(cy_),
 47 |           _observation_x(observation_x),
 48 |           _observation_y(observation_y){}
 49 | 
 50 |     virtual bool Evaluate(double const* const* parameters,
 51 |                           double* residuals,
 52 |                           double** jacobians) const;
 53 | 
 54 |     double f;
 55 |     double cx;
 56 |     double cy;
 57 | 
 58 | private:
 59 |     double _observation_x;
 60 |     double _observation_y;
 61 | };
 62 | 
 63 | /// PoseBlockSize can only be
 64 | /// 7 (quaternion + translation vector) or
 65 | /// 6 (rotation vector + translation vector)
 66 | template<int PoseBlockSize>
 67 | class PoseSE3Parameterization : public ceres::LocalParameterization {
 68 | public:
 69 |     PoseSE3Parameterization() {}
 70 |     virtual ~PoseSE3Parameterization() {}
 71 |     virtual bool Plus(const double* x,
 72 |                       const double* delta,
 73 |                       double* x_plus_delta) const;
 74 |     virtual bool ComputeJacobian(const double* x,
 75 |                                  double* jacobian) const;
 76 |     virtual int GlobalSize() const { return PoseBlockSize; }
 77 |     virtual int LocalSize() const { return 6; }
 78 | };
 79 | 
 80 | /// PoseBlockSize can only be
 81 | /// 7 (quaternion + translation vector) or
 82 | /// 6 (rotation vector + translation vector)
 83 | template<int PoseBlockSize>
 84 | class PosePointParametersBlock
 85 | {
 86 | public:
 87 |     PosePointParametersBlock(){}
 88 |     void create(int pose_num, int point_num)
 89 |     {
 90 |         poseNum = pose_num;
 91 |         pointNum = point_num;
 92 |         values = new double[pose_num * PoseBlockSize + point_num * 3];
 93 |     }
 94 |     PosePointParametersBlock(int pose_num, int point_num): poseNum(pose_num), pointNum(point_num)
 95 |     {
 96 |         values = new double[pose_num * PoseBlockSize + point_num * 3];
 97 |     }
 98 |     ~PosePointParametersBlock() { delete[] values; }
 99 | 
100 |     void setPose(int idx, const Quaterniond &q, const Vector3d &trans);
101 | 
102 |     void getPose(int idx, Quaterniond &q, Vector3d &trans);
103 | 
104 |     double* pose(int idx) {  return values + idx * PoseBlockSize; }
105 | 
106 |     double* point(int idx) { return values + poseNum * PoseBlockSize + idx * 3; }
107 | 
108 |     int poseNum;
109 |     int pointNum;
110 |     double *values;
111 | 
112 | };
113 | 
114 | 
115 | #endif // PARAMETERSSE3_HPP
116 | 


--------------------------------------------------------------------------------
/baproblem.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef BAPROBLEM_HPP_
  2 | #define BAPROBLEM_HPP_
  3 | 
  4 | #include "parametersse3.hpp"
  5 | 
  6 | class Sample
  7 | {
  8 | public:
  9 |     static int uniform(int from, int to);
 10 |     static double uniform();
 11 |     static double gaussian(double sigma);
 12 | };
 13 | 
 14 | /// PoseBlockSize can only be
 15 | /// 7 (quaternion + translation vector) or
 16 | /// 6 (rotation vector + translation vector)
 17 | template <int PoseBlockSize>
 18 | class BAProblem
 19 | {
 20 | public:
 21 |     BAProblem(int pose_num_, int point_num_, double pix_noise_, bool useOrdering = false);
 22 | 
 23 |     void solve(ceres::Solver::Options &opt, ceres::Solver::Summary* sum);
 24 | 
 25 |     ceres::Problem problem;
 26 |     ceres::ParameterBlockOrdering* ordering = NULL;
 27 | 
 28 | protected:
 29 |     PosePointParametersBlock<PoseBlockSize> states;
 30 |     PosePointParametersBlock<PoseBlockSize> true_states;
 31 | 
 32 | };
 33 | 
 34 | 
 35 | template<int PoseBlockSize>
 36 | BAProblem<PoseBlockSize>::BAProblem(int pose_num_, int point_num_, double pix_noise_, bool useOrdering)
 37 | {
 38 |     if(useOrdering)
 39 |         ordering = new ceres::ParameterBlockOrdering;
 40 | 
 41 |     int pose_num = pose_num_;
 42 |     int point_num = point_num_;
 43 |     double PIXEL_NOISE = pix_noise_;
 44 | 
 45 |     states.create(pose_num, point_num);
 46 |     true_states.create(pose_num, point_num);
 47 | 
 48 |     for (int i = 0; i < point_num; ++i)
 49 |     {
 50 |         Eigen::Map<Vector3d> true_pt(true_states.point(i));
 51 |         true_pt = Vector3d((Sample::uniform() - 0.5) * 3,
 52 |                            Sample::uniform() - 0.5,
 53 |                            Sample::uniform() + 3);
 54 |     }
 55 | 
 56 |     double focal_length = 1000.;
 57 |     double cx = 320.;
 58 |     double cy = 240.;
 59 |     CameraParameters cam(focal_length, cx, cy);
 60 | 
 61 |     for (int i = 0; i < pose_num; ++i)
 62 |     {
 63 |         Vector3d trans(i * 0.04 - 1., 0, 0);
 64 | 
 65 |         Eigen::Quaterniond q;
 66 |         q.setIdentity();
 67 |         true_states.setPose(i, q, trans);
 68 |         states.setPose(i, q, trans);
 69 | 
 70 |         problem.AddParameterBlock(states.pose(i), PoseBlockSize, new PoseSE3Parameterization<PoseBlockSize>());
 71 | 
 72 |         if(i < 2)
 73 |         {
 74 |             problem.SetParameterBlockConstant(states.pose(i));
 75 |         }
 76 |     }
 77 | 
 78 |     for (int i = 0; i < point_num; ++i)
 79 |     {
 80 |         Eigen::Map<Vector3d> true_point_i(true_states.point(i));
 81 |         Eigen::Map<Vector3d> noise_point_i(states.point(i));
 82 |         noise_point_i = true_point_i + Vector3d(Sample::gaussian(1),
 83 |                                                 Sample::gaussian(1),
 84 |                                                 Sample::gaussian(1));
 85 | 
 86 |         Vector2d z;
 87 |         SE3 true_pose_se3;
 88 | 
 89 |         int num_obs = 0;
 90 |         for (int j = 0; j < pose_num; ++j)
 91 |         {
 92 |             true_states.getPose(j, true_pose_se3.rotation(), true_pose_se3.translation());
 93 |             Vector3d point_cam = true_pose_se3.map(true_point_i);
 94 |             z = cam.cam_map(point_cam);
 95 |             if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480)
 96 |             {
 97 |                 ++num_obs;
 98 |             }
 99 |         }
100 |         if (num_obs >= 2)
101 |         {
102 |             problem.AddParameterBlock(states.point(i), 3);
103 |             if(useOrdering)
104 |                 ordering->AddElementToGroup(states.point(i), 0);
105 | 
106 |             for (int j = 0; j < pose_num; ++j)
107 |             {
108 |                 true_states.getPose(j, true_pose_se3.rotation(), true_pose_se3.translation());
109 |                 Vector3d point_cam = true_pose_se3.map(true_point_i);
110 |                 z = cam.cam_map(point_cam);
111 | 
112 |                 if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480)
113 |                 {
114 |                     z += Vector2d(Sample::gaussian(PIXEL_NOISE),
115 |                                   Sample::gaussian(PIXEL_NOISE));
116 | 
117 |                     ceres::CostFunction* costFunc = new ReprojectionErrorSE3XYZ<PoseBlockSize>(focal_length, cx, cy, z[0], z[1]);
118 |                     problem.AddResidualBlock(costFunc, NULL, states.pose(j), states.point(i));
119 |                 }
120 |             }
121 | 
122 |         }
123 |     }
124 | 
125 |     if(useOrdering)
126 |         for (int i = 0; i < pose_num; ++i)
127 |         {
128 |             ordering->AddElementToGroup(states.pose(i), 1);
129 |         }
130 | 
131 | }
132 | 
133 | 
134 | template<int PoseBlockSize>
135 | void BAProblem<PoseBlockSize>::solve(ceres::Solver::Options& opt, ceres::Solver::Summary *sum)
136 | {
137 |     if(ordering != NULL)
138 |         opt.linear_solver_ordering.reset(ordering);
139 |     ceres::Solve(opt, &problem, sum);
140 | }
141 | 
142 | #endif
143 | 


--------------------------------------------------------------------------------
/parametersse3.cpp:
--------------------------------------------------------------------------------
  1 | #include "parametersse3.hpp"
  2 | 
  3 | template<>
  4 | bool ReprojectionErrorSE3XYZ<7>::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const
  5 | {
  6 |     Eigen::Map<const Quaterniond> quaterd(parameters[0]);
  7 |     Eigen::Map<const Eigen::Vector3d> trans(parameters[0] + 4);
  8 |     Eigen::Map<const Eigen::Vector3d> point(parameters[1]);
  9 | 
 10 |     Eigen::Vector3d p = quaterd * point + trans;
 11 | 
 12 |     double f_by_z = f / p[2];
 13 |     residuals[0] = f_by_z * p[0] + cx - _observation_x;
 14 |     residuals[1] = f_by_z * p[1] + cy - _observation_y;
 15 | 
 16 |     Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_cam;
 17 |     double f_by_zz = f_by_z / p[2];
 18 |     J_cam << f_by_z, 0, - f_by_zz * p[0],
 19 |             0, f_by_z, - f_by_zz * p[1];
 20 | 
 21 | 
 22 |     if(jacobians != NULL)
 23 |     {
 24 |         if(jacobians[0] != NULL)
 25 |         {
 26 |             Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
 27 |             J_se3.setZero();
 28 |             J_se3.block<2,3>(0,0) = - J_cam * skew(p);
 29 |             J_se3.block<2,3>(0,3) = J_cam;
 30 |         }
 31 |         if(jacobians[1] != NULL)
 32 |         {
 33 |             Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor> > J_point(jacobians[1]);
 34 |             J_point = J_cam * quaterd.toRotationMatrix();
 35 |         }
 36 |     }
 37 | 
 38 |     return true;
 39 | }
 40 | 
 41 | template<>
 42 | bool ReprojectionErrorSE3XYZ<6>::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const
 43 | {
 44 |     Eigen::Quaterniond quaterd = toQuaterniond(Eigen::Map<const Vector3d>(parameters[0]));
 45 |     Eigen::Map<const Eigen::Vector3d> trans(parameters[0] + 3);
 46 |     Eigen::Map<const Eigen::Vector3d> point(parameters[1]);
 47 | 
 48 |     Eigen::Vector3d p = quaterd * point + trans;
 49 | 
 50 |     double f_by_z = f / p[2];
 51 |     residuals[0] = f_by_z * p[0] + cx - _observation_x;
 52 |     residuals[1] = f_by_z * p[1] + cy - _observation_y;
 53 | 
 54 |     Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_cam;
 55 |     double f_by_zz = f_by_z / p[2];
 56 |     J_cam << f_by_z, 0, - f_by_zz * p[0],
 57 |             0, f_by_z, - f_by_zz * p[1];
 58 | 
 59 |     if(jacobians != NULL)
 60 |     {
 61 |         if(jacobians[0] != NULL)
 62 |         {
 63 |             Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J_se3(jacobians[0]);
 64 |             J_se3.block<2,3>(0,0) = - J_cam * skew(p);
 65 |             J_se3.block<2,3>(0,3) = J_cam;
 66 |         }
 67 |         if(jacobians[1] != NULL)
 68 |         {
 69 |             Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor> > J_point(jacobians[1]);
 70 |             J_point = J_cam * quaterd.toRotationMatrix();
 71 |         }
 72 |     }
 73 | 
 74 |     return true;
 75 | }
 76 | 
 77 | 
 78 | template<>
 79 | bool PoseSE3Parameterization<7>::Plus(const double *x, const double *delta, double *x_plus_delta) const
 80 | {
 81 |     Eigen::Map<const Eigen::Vector3d> trans(x + 4);
 82 |     SE3 se3_delta = SE3::exp(Eigen::Map<const Vector6d>(delta));
 83 | 
 84 |     Eigen::Map<const Eigen::Quaterniond> quaterd(x);
 85 |     Eigen::Map<Eigen::Quaterniond> quaterd_plus(x_plus_delta);
 86 |     Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);
 87 | 
 88 |     quaterd_plus = se3_delta.rotation() * quaterd;
 89 |     trans_plus = se3_delta.rotation() * trans + se3_delta.translation();
 90 | 
 91 |     return true;
 92 | }
 93 | 
 94 | template<>
 95 | bool PoseSE3Parameterization<7>::ComputeJacobian(const double *x, double *jacobian) const
 96 | {
 97 |     Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor> > J(jacobian);
 98 |     J.setZero();
 99 |     J.block<6,6>(0, 0).setIdentity();
100 |     return true;
101 | }
102 | 
103 | 
104 | 
105 | template<>
106 | bool PoseSE3Parameterization<6>::Plus(const double *x, const double *delta, double *x_plus_delta) const
107 | {
108 |     Eigen::Map<const Eigen::Vector3d> trans(x + 3);
109 |     SE3 se3_delta = SE3::exp(Eigen::Map<const Vector6d>(delta));
110 | 
111 |     Quaterniond quaterd_plus = se3_delta.rotation() * toQuaterniond(Eigen::Map<const Vector3d>(x));
112 |     Eigen::Map<Vector3d> angles_plus(x_plus_delta);
113 |     angles_plus = toAngleAxis(quaterd_plus);
114 | 
115 |     Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 3);
116 |     trans_plus = se3_delta.rotation() * trans + se3_delta.translation();
117 |     return true;
118 | }
119 | 
120 | template<>
121 | bool PoseSE3Parameterization<6>::ComputeJacobian(const double *x, double *jacobian) const
122 | {
123 |     Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > J(jacobian);
124 |     J.setIdentity();
125 |     return true;
126 | }
127 | 
128 | 
129 | 
130 | template<>
131 | void PosePointParametersBlock<7>::getPose(int idx, Quaterniond &q, Vector3d &trans)
132 | {
133 |     double* pose_ptr = values + idx * 7;
134 |     q = Map<const Quaterniond>(pose_ptr);
135 |     trans = Map<const Vector3d>(pose_ptr + 4);
136 | }
137 | 
138 | template<>
139 | void PosePointParametersBlock<7>::setPose(int idx, const Quaterniond &q, const Vector3d &trans)
140 | {
141 |     double* pose_ptr = values + idx * 7;
142 |     Eigen::Map<Vector7d> pose(pose_ptr);
143 |     pose.head<4>() = Eigen::Vector4d(q.coeffs());
144 |     pose.tail<3>() = trans;
145 | }
146 | 
147 | template<>
148 | void PosePointParametersBlock<6>::getPose(int idx, Quaterniond &q, Vector3d &trans)
149 | {
150 |     double* pose_ptr = values + idx * 6;
151 |     q = toQuaterniond(Vector3d(pose_ptr));
152 |     trans = Map<const Vector3d>(pose_ptr + 3);
153 | }
154 | 
155 | template<>
156 | void PosePointParametersBlock<6>::setPose(int idx, const Quaterniond &q, const Vector3d &trans)
157 | {
158 |     double* pose_ptr = values + idx * 6;
159 |     Eigen::Map<Vector6d> pose(pose_ptr);
160 |     pose.head<3>() = toAngleAxis(q);
161 |     pose.tail<3>() = trans;
162 | }
163 | 


--------------------------------------------------------------------------------
/se3.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SE3_HPP_
  2 | #define SE3_HPP_
  3 | 
  4 | #include <Eigen/Core>
  5 | #include <Eigen/Geometry>
  6 | #ifdef _MSC_VER
  7 | #define _USE_MATH_DEFINES
  8 | #endif
  9 | #include <math.h>
 10 | 
 11 | const double SMALL_EPS = 1e-10;
 12 | 
 13 | typedef Eigen::Matrix<double, 6, 1, Eigen::ColMajor> Vector6d;
 14 | typedef Eigen::Matrix<double, 7, 1, Eigen::ColMajor> Vector7d;
 15 | 
 16 | inline Eigen::Matrix3d skew(const Eigen::Vector3d&v)
 17 | {
 18 |     Eigen::Matrix3d m;
 19 |     m.fill(0.);
 20 |     m(0,1)  = -v(2);
 21 |     m(0,2)  =  v(1);
 22 |     m(1,2)  = -v(0);
 23 |     m(1,0)  =  v(2);
 24 |     m(2,0) = -v(1);
 25 |     m(2,1) = v(0);
 26 |     return m;
 27 | }
 28 | 
 29 | inline Eigen::Vector3d deltaR(const Eigen::Matrix3d& R)
 30 | {
 31 |     Eigen::Vector3d v;
 32 |     v(0)=R(2,1)-R(1,2);
 33 |     v(1)=R(0,2)-R(2,0);
 34 |     v(2)=R(1,0)-R(0,1);
 35 |     return v;
 36 | }
 37 | 
 38 | 
 39 | inline Eigen::Vector3d toAngleAxis(const Eigen::Quaterniond& quaterd, double* angle=NULL)
 40 | {
 41 |     Eigen::Quaterniond unit_quaternion = quaterd.normalized();
 42 |     double n = unit_quaternion.vec().norm();
 43 |     double w = unit_quaternion.w();
 44 |     double squared_w = w*w;
 45 | 
 46 |     double two_atan_nbyw_by_n;
 47 |     // Atan-based log thanks to
 48 |     //
 49 |     // C. Hertzberg et al.:
 50 |     // "Integrating Generic Sensor Fusion Algorithms with Sound State
 51 |     // Representation through Encapsulation of Manifolds"
 52 |     // Information Fusion, 2011
 53 | 
 54 |     if (n < SMALL_EPS)
 55 |     {
 56 |         // If quaternion is normalized and n=1, then w should be 1;
 57 |         // w=0 should never happen here!
 58 |         assert(fabs(w)>SMALL_EPS);
 59 | 
 60 |         two_atan_nbyw_by_n = 2./w - 2.*(n*n)/(w*squared_w);
 61 |     }
 62 |     else
 63 |     {
 64 |         if (fabs(w)<SMALL_EPS)
 65 |         {
 66 |             if (w>0)
 67 |             {
 68 |                 two_atan_nbyw_by_n = M_PI/n;
 69 |             }
 70 |             else
 71 |             {
 72 |                 two_atan_nbyw_by_n = -M_PI/n;
 73 |             }
 74 |         }
 75 |         two_atan_nbyw_by_n = 2*atan(n/w)/n;
 76 |     }
 77 |     if(angle!=NULL) *angle = two_atan_nbyw_by_n*n;
 78 |     return two_atan_nbyw_by_n * unit_quaternion.vec();
 79 | }
 80 | 
 81 | inline Eigen::Quaterniond toQuaterniond(const Eigen::Vector3d& v3d, double* angle = NULL)
 82 | {
 83 |     double theta = v3d.norm();
 84 |     if(angle != NULL)
 85 |         *angle = theta;
 86 |     double half_theta = 0.5*theta;
 87 | 
 88 |     double imag_factor;
 89 |     double real_factor = cos(half_theta);
 90 |     if(theta<SMALL_EPS)
 91 |     {
 92 |         double theta_sq = theta*theta;
 93 |         double theta_po4 = theta_sq*theta_sq;
 94 |         imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
 95 |     }
 96 |     else
 97 |     {
 98 |         double sin_half_theta = sin(half_theta);
 99 |         imag_factor = sin_half_theta/theta;
100 |     }
101 | 
102 |     return Eigen::Quaterniond(real_factor,
103 |                               imag_factor*v3d.x(),
104 |                               imag_factor*v3d.y(),
105 |                               imag_factor*v3d.z());
106 | }
107 | 
108 | 
109 | class SE3 {
110 | public:
111 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
112 | 
113 | protected:
114 | 
115 |     Eigen::Quaterniond _r;
116 |     Eigen::Vector3d _t;
117 | 
118 | public:
119 |     SE3(){
120 |         _r.setIdentity();
121 |         _t.setZero();
122 |     }
123 | 
124 |     SE3(const Eigen::Matrix3d& R, const Eigen::Vector3d& t):_r(Eigen::Quaterniond(R)),_t(t){
125 |         normalizeRotation();
126 |     }
127 | 
128 |     SE3(const Eigen::Quaterniond& q, const Eigen::Vector3d& t):_r(q),_t(t){
129 |         normalizeRotation();
130 |     }
131 | 
132 |     inline const Eigen::Vector3d& translation() const {return _t;}
133 | 
134 |     inline Eigen::Vector3d& translation() {return _t;}
135 | 
136 |     inline void setTranslation(const Eigen::Vector3d& t_) {_t = t_;}
137 | 
138 |     inline const Eigen::Quaterniond& rotation() const {return _r;}
139 | 
140 |     inline Eigen::Quaterniond& rotation() {return _r;}
141 | 
142 |     void setRotation(const Eigen::Quaterniond& r_) {_r=r_;}
143 | 
144 |     inline SE3 operator* (const SE3& tr2) const{
145 |         SE3 result(*this);
146 |         result._t += _r*tr2._t;
147 |         result._r*=tr2._r;
148 |         result.normalizeRotation();
149 |         return result;
150 |     }
151 | 
152 |     inline SE3& operator*= (const SE3& tr2){
153 |         _t+=_r*tr2._t;
154 |         _r*=tr2._r;
155 |         normalizeRotation();
156 |         return *this;
157 |     }
158 | 
159 |     inline Eigen::Vector3d operator* (const Eigen::Vector3d& v) const {
160 |         return _t+_r*v;
161 |     }
162 | 
163 |     inline SE3 inverse() const{
164 |         SE3 ret;
165 |         ret._r=_r.conjugate();
166 |         ret._t=ret._r*(_t*-1.);
167 |         return ret;
168 |     }
169 | 
170 |     inline double operator [](int i) const {
171 |         assert(i<7);
172 |         if (i<4)
173 |             return _r.coeffs()[i];
174 |         return _t[i-4];
175 |     }
176 | 
177 | 
178 |     inline Vector7d toVector() const{
179 |         Vector7d v;
180 |         v.head<4>() = Eigen::Vector4d(_r.coeffs());
181 |         v.tail<3>() = _t;
182 |         return v;
183 |     }
184 | 
185 |     inline void fromVector(const Vector7d& v){
186 |         _r=Eigen::Quaterniond(v[3], v[0], v[1], v[2]);
187 |         _t=Eigen::Vector3d(v[4], v[5], v[6]);
188 |     }
189 | 
190 | 
191 |     Vector6d log() const {
192 |         Vector6d res;
193 | 
194 |         double theta;
195 |         res.head<3>() = toAngleAxis(_r, &theta);
196 | 
197 |         Eigen::Matrix3d Omega = skew(res.head<3>());
198 |         Eigen::Matrix3d V_inv;
199 |         if (theta<SMALL_EPS)
200 |         {
201 |             V_inv = Eigen::Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
202 |         }
203 |         else
204 |         {
205 |             V_inv = ( Eigen::Matrix3d::Identity() - 0.5*Omega
206 |                       + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
207 |         }
208 | 
209 |         res.tail<3>() = V_inv*_t;
210 | 
211 |         return res;
212 |     }
213 | 
214 |     Eigen::Vector3d map(const Eigen::Vector3d & xyz) const
215 |     {
216 |         return _r*xyz + _t;
217 |     }
218 | 
219 | 
220 |     static SE3 exp(const Vector6d & update)
221 |     {
222 |         Eigen::Vector3d omega(update.data());
223 |         Eigen::Vector3d upsilon(update.data()+3);
224 | 
225 |         double theta;
226 |         Eigen::Matrix3d Omega = skew(omega);
227 | 
228 |         Eigen::Quaterniond R = toQuaterniond(omega, &theta);
229 |         Eigen::Matrix3d V;
230 |         if (theta<SMALL_EPS)
231 |         {
232 |             V = R.matrix();
233 |         }
234 |         else
235 |         {
236 |             Eigen::Matrix3d Omega2 = Omega*Omega;
237 | 
238 |             V = (Eigen::Matrix3d::Identity()
239 |                  + (1-cos(theta))/(theta*theta)*Omega
240 |                  + (theta-sin(theta))/(pow(theta,3))*Omega2);
241 |         }
242 |         return SE3(R, V*upsilon);
243 |     }
244 | 
245 |     Eigen::Matrix<double, 6, 6, Eigen::ColMajor> adj() const
246 |     {
247 |         Eigen::Matrix3d R = _r.toRotationMatrix();
248 |         Eigen::Matrix<double, 6, 6, Eigen::ColMajor> res;
249 |         res.block(0,0,3,3) = R;
250 |         res.block(3,3,3,3) = R;
251 |         res.block(3,0,3,3) = skew(_t)*R;
252 |         res.block(0,3,3,3) = Eigen::Matrix3d::Zero(3,3);
253 |         return res;
254 |     }
255 | 
256 |     Eigen::Matrix<double,4,4,Eigen::ColMajor> to_homogeneous_matrix() const
257 |     {
258 |         Eigen::Matrix<double,4,4,Eigen::ColMajor> homogeneous_matrix;
259 |         homogeneous_matrix.setIdentity();
260 |         homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
261 |         homogeneous_matrix.col(3).head(3) = translation();
262 | 
263 |         return homogeneous_matrix;
264 |     }
265 | 
266 |     void normalizeRotation(){
267 |         if (_r.w()<0){
268 |             _r.coeffs() *= -1;
269 |         }
270 |         _r.normalize();
271 |     }
272 | };
273 | 
274 | #endif // SE3_HPP_
275 | 


--------------------------------------------------------------------------------