├── doc
    ├── 1DSfM.jpg
    ├── KITTI.jpg
    └── large_scale.jpg
├── include
    └── STBA
    │   ├── test.h
    │   ├── commandhandler.h
    │   ├── clustering
    │       ├── graph.h
    │       └── louvain.h
    │   ├── lmbaproblem.h
    │   ├── dlbaproblem.h
    │   ├── lossfunction.tpp
    │   ├── lossfunction.h
    │   ├── stochasticbaproblem.h
    │   ├── utility.h
    │   ├── baproblem.h
    │   └── datablock.h
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── src
    ├── clustering
    │   ├── graph.cpp
    │   └── louvain.cpp
    ├── lossfunction.cpp
    ├── test.cpp
    ├── main.cpp
    ├── lmbaproblem.cpp
    ├── utility.cpp
    ├── dlbaproblem.cpp
    ├── datablock.cpp
    └── stochasticbaproblem.cpp
└── README.md


/doc/1DSfM.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zlthinker/STBA/HEAD/doc/1DSfM.jpg


--------------------------------------------------------------------------------
/doc/KITTI.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zlthinker/STBA/HEAD/doc/KITTI.jpg


--------------------------------------------------------------------------------
/doc/large_scale.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zlthinker/STBA/HEAD/doc/large_scale.jpg


--------------------------------------------------------------------------------
/include/STBA/test.h:
--------------------------------------------------------------------------------
1 | #ifndef TEST_H
2 | #define TEST_H
3 | 
4 | void TestLM();
5 | 
6 | 
7 | #endif
8 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | *.txt~
 2 | *.cpp~
 3 | *.tpp~
 4 | *.h~
 5 | CMakeLists.txt.user
 6 | build/
 7 | test/
 8 | .gitignore
 9 | .vscode/
10 | GPATH
11 | GRTAGS
12 | GTAGS
13 | 


--------------------------------------------------------------------------------
/include/STBA/commandhandler.h:
--------------------------------------------------------------------------------
 1 | #ifndef COMMANDHANDLER_H
 2 | #define COMMANDHANDLER_H
 3 | 
 4 | #include <string>
 5 | #include <vector>
 6 | 
 7 | #if defined(__cplusplus)
 8 | extern "C" {
 9 | #endif
10 | 
11 | bool commandHandler(std::vector<std::string> & commandOptions);
12 | void commandHelp();
13 | 
14 | #if defined(__cplusplus)
15 | }
16 | #endif
17 | 
18 | #endif // COMMANDHANDLER_H
19 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.7)
 2 | project(STBA)
 3 | 
 4 | find_package(OpenMP)
 5 | if (OPENMP_FOUND)
 6 |     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
 7 |     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 8 |     message(STATUS "OpenMP is found")
 9 | else ()
10 |     message(STATUS "OpenMP is not found")
11 | endif()
12 | 
13 | find_package(Eigen3 REQUIRED)
14 | include_directories(${EIGEN3_INCLUDE_DIR})
15 | 
16 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2 -std=c++11")
17 | 
18 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
19 | 
20 | file(GLOB SOURCES "src/*.cpp")
21 | file(GLOB SUB_SOURCES "src/clustering/*.cpp")
22 | set(SOURCES ${SOURCES} ${SUB_SOURCES})
23 | 
24 | 
25 | add_executable(STBA ${SOURCES})
26 | target_compile_definitions(STBA PUBLIC STBA_VERSION="0.0.0")
27 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Lei Zhou
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/include/STBA/clustering/graph.h:
--------------------------------------------------------------------------------
 1 | #ifndef _NETWORK_HEADER
 2 | #define _NETWORK_HEADER
 3 | 
 4 | #include "STBA/utility.h"
 5 | #include <vector>
 6 | 
 7 | struct EdgeData
 8 | {
 9 |     EdgeData() : node(-1), weight(0.0) {}
10 |     EdgeData(size_t n, double w) : node(n), weight(w) {}
11 | 
12 |     size_t node;
13 |     double weight;
14 | };
15 | 
16 | struct Graph
17 | {
18 |     Graph();
19 | 
20 |     void AddSelfEdge(size_t i, double weight = 0.0);
21 |     void AddUndirectedEdge(size_t i, size_t j, double weight = 1.0);
22 |     void AddDirectedEdge(size_t i, size_t j, double weight = 1.0);
23 | 
24 |     size_t EdgeNum() const;
25 |     double EdgeWeight(size_t, size_t) const;
26 | 
27 |     inline size_t NodeNum() const { return incidences_.size(); }
28 |     inline const std::vector<EdgeData>& GetIncidentEdges(int i) const { return incidences_[i]; }
29 |     inline double GetSelfWeight(int i) const { return selfs_[i]; }
30 |     inline void Swap(Graph& other)
31 |     {
32 |         other.selfs_.swap(selfs_);
33 |         other.incidences_.swap(incidences_);
34 |     }
35 |     void Resize(size_t num)
36 |     {
37 |         selfs_.resize(num);
38 |         incidences_.resize(num);
39 |     }
40 | 
41 |     std::vector<double> selfs_;
42 |     std::vector<std::vector<EdgeData> > incidences_;
43 | };
44 | 
45 | #endif
46 | 


--------------------------------------------------------------------------------
/include/STBA/lmbaproblem.h:
--------------------------------------------------------------------------------
 1 | #ifndef LMBAPROBLAM_H
 2 | #define LMBAPROBLAM_H
 3 | 
 4 | #include "baproblem.h"
 5 | 
 6 | class LMBAProblem : public BAProblem
 7 | {
 8 | public:
 9 |     LMBAProblem();
10 |     LMBAProblem(LossType loss_type);
11 |     LMBAProblem(size_t max_iter, double radius, LossType loss_type);
12 |     LMBAProblem(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num);
13 |     virtual ~LMBAProblem();
14 | 
15 | public:
16 |     virtual void Solve();
17 |     inline void SetRadius(double r) { mu_ = r; }
18 |     inline void SetMaxIteration(size_t iter) { max_iteration_ = iter; }
19 | 
20 | protected:
21 |     void DecreaseRadius();  // When step rejected
22 |     void IncreaseRadius();  // When step accepted
23 |     void EvaluateRho(VecX const &);
24 |     bool StopCriterionGradient();
25 |     bool StopCriterionUpdate();
26 |     bool StopCriterionRadius();
27 |     bool StopCriterionRelativeCostChange();
28 |     virtual void Print();
29 |     double Step() const;
30 |     double MaxGradient() const;
31 |     bool StepAccept() const;
32 | 
33 |     void AugmentPoseDiagonal();
34 |     void ResetPoseDiagonal();
35 |     virtual void AugmentPointDiagonal();
36 |     virtual void ResetPointDiagonal();
37 | 
38 | 
39 | protected:
40 |     size_t max_iteration_;
41 |     double mu_;     // 1/lambda
42 |     double rho_;    // gain ratio
43 |     double decrease_factor_;
44 |     double last_square_error_;
45 |     double square_error_;
46 |     VecX pose_diagonal_;
47 |     VecX intrinsic_diagonal_;
48 |     VecX point_diagonal_;
49 |     bool evaluate_;
50 | };
51 | 
52 | #endif // LMBAPROBLAM_H
53 | 


--------------------------------------------------------------------------------
/src/clustering/graph.cpp:
--------------------------------------------------------------------------------
 1 | #include <cassert>
 2 | #include <iostream>
 3 | 
 4 | #include "STBA/clustering/graph.h"
 5 | 
 6 | Graph::Graph()
 7 | {
 8 | }
 9 | 
10 | void Graph::AddSelfEdge(size_t i, double weight)
11 | {
12 |     assert(i < selfs_.size());
13 |     selfs_[i] = weight;
14 | }
15 | 
16 | void Graph::AddUndirectedEdge(size_t i, size_t j, double weight)
17 | {
18 | 	assert(i != j);
19 |     assert(i < incidences_.size() && j < incidences_.size());
20 | 	// Do not check if an edge is added more than once
21 |     incidences_[i].push_back({j, weight});
22 |     incidences_[j].push_back({i, weight});
23 | }
24 | 
25 | void Graph::AddDirectedEdge(size_t i, size_t j, double weight)
26 | {
27 | 	assert(i != j);
28 |     assert(i < incidences_.size() && j < incidences_.size());
29 | 	// Do not check if an edge is added more than once
30 |     incidences_[i].push_back({j, weight});
31 | }
32 | 
33 | size_t Graph::EdgeNum() const
34 | {
35 |     size_t node_num = NodeNum();
36 |     size_t edge_num = 0;
37 |     for (size_t i = 0; i < node_num; i++)
38 |     {
39 |         edge_num += incidences_[i].size();
40 |     }
41 |     return edge_num / 2;
42 | }
43 | 
44 | double Graph::EdgeWeight(size_t i, size_t j) const
45 | {
46 |     assert(i != j);
47 |     assert(i < incidences_.size() && j < incidences_.size());
48 |     double weight = 0.0;
49 |     std::vector<EdgeData> const & edges = incidences_[i];
50 |     bool found = false;
51 |     for (size_t k = 0; k < edges.size(); k++)
52 |     {
53 |         EdgeData const & edge = edges[k];
54 |         if (edge.node == j)
55 |         {
56 |             weight = edge.weight;
57 |             found = true;
58 |             break;
59 |         }
60 |     }
61 |     assert(found && "Edge not exists");
62 |     return weight;
63 | }
64 | 


--------------------------------------------------------------------------------
/include/STBA/dlbaproblem.h:
--------------------------------------------------------------------------------
 1 | #ifndef DLBAPROBLEM_H
 2 | #define DLBAPROBLEM_H
 3 | 
 4 | #include "baproblem.h"
 5 | 
 6 | class DLBAProblem : public BAProblem
 7 | {
 8 | public:
 9 |     DLBAProblem();
10 |     DLBAProblem(LossType loss_type);
11 |     DLBAProblem(size_t max_iter, double radius, LossType loss_type);
12 |     virtual ~DLBAProblem();
13 | 
14 | protected:
15 |     void EvaluateGradient();
16 |     bool EvaluateCauchyStep();
17 |     bool EvaluateGaussNewtonStep();
18 |     bool EvaluateDogLegStep();
19 |     bool EvaluateStep();
20 |     void EvaluateRho();
21 |     bool StepAccept();
22 |     virtual void Solve();
23 | 
24 |     void GetPoseDiagonal();
25 |     void GetPointDiagonal();
26 |     void AugmentPoseDiagonal();
27 |     void ResetPoseDiagonal();
28 |     void AugmentPointDiagonal();
29 |     void ResetPointDiagonal();
30 |     double Step() const;
31 |     bool StopCriterionUpdate();
32 |     bool StopCriterionRelativeCostChange();
33 |     inline void SetRadius(double r) { radius_ = r; }
34 |     inline void SetMaxIteration(size_t iter) { max_iteration_ = iter; }
35 |     void Print();
36 | 
37 | private:
38 |     size_t max_iteration_;
39 |     double mu_;
40 |     double min_mu_;
41 |     double max_mu_;
42 |     double radius_;
43 |     double alpha_;                          // factor of the cauchy step
44 |     double rho_;                             // step quality
45 |     VecX pose_gradient_;
46 |     VecX point_gradient_;
47 |     VecX pose_diagonal_;
48 |     VecX point_diagonal_;
49 |     VecX pose_diagonal_sqrt_;
50 |     VecX point_diagonal_sqrt_;
51 |     VecX cauchy_step_;
52 |     VecX gauss_newton_step_;
53 |     VecX dl_step_;
54 |     double dl_step_norm_;
55 |     bool evaluate_;
56 |     double square_error_;
57 |     double last_square_error_;
58 | };
59 | 
60 | #endif // DLBAPROBLEM_H
61 | 


--------------------------------------------------------------------------------
/include/STBA/lossfunction.tpp:
--------------------------------------------------------------------------------
 1 | #ifndef LOSSFUNCTION_TPP
 2 | #define LOSSFUNCTION_TPP
 3 | 
 4 | #include "lossfunction.h"
 5 | 
 6 | template<size_t R, size_t C>
 7 | void LossFunction::CorrectJacobian(Vec2 const & residual,
 8 |                                    Matrix<DT, R, C, RowMajor> & jacobian) const
 9 | {
10 |     size_t num_rows = jacobian.rows();  // #residuals
11 |     size_t num_cols = jacobian.cols();  // #parameters
12 |     assert(residual.size() == num_rows && "[CorrectJacobian] Dimension disagrees.");
13 | 
14 |     double sq_norm = residual.squaredNorm();
15 |     if (sq_norm > MAX_VALID_ERROR)
16 |     {
17 |         jacobian = Matrix<DT, R, C, RowMajor>::Zero();
18 |         return;
19 |     }
20 | 
21 |     double rho1 = FirstOrderDerivative(sq_norm);
22 |     double rho2 = SecondOrderDerivative(sq_norm);
23 |     double sqrt_rho1 = std::sqrt(rho1);
24 | 
25 |     double residual_scaling, alpha_sq_norm;
26 |     if ((sq_norm == 0.0) || (rho2 <= 0.0))
27 |     {
28 |         residual_scaling = sqrt_rho1;
29 |         alpha_sq_norm = 0.0;
30 |     }
31 |     else
32 |     {
33 |         assert(rho1 > 0.0 && "[CorrectResiduals] First order derivative mush be positive");
34 |         double D = 1.0 + 2.0 * sq_norm * rho2 / rho1;
35 |         double alpha = 1.0 - std::sqrt(D);
36 |         residual_scaling = sqrt_rho1 / (1.0 - alpha);
37 |         alpha_sq_norm = alpha / sq_norm;
38 |     }
39 | 
40 |     if (alpha_sq_norm == 0.0)
41 |     {
42 |         jacobian *= sqrt_rho1;
43 |     }
44 |     else
45 |     {
46 |         MatX residual_transpose_J = residual.transpose() * jacobian;
47 |         MatX residual_square_J = residual * residual_transpose_J;
48 |         residual_square_J *= alpha_sq_norm;
49 |         jacobian = sqrt_rho1 * (jacobian - residual_square_J);
50 |     }
51 | }
52 | 
53 | #endif // LOSSFUNCTION_TPP
54 | 
55 | 


--------------------------------------------------------------------------------
/include/STBA/lossfunction.h:
--------------------------------------------------------------------------------
 1 | #ifndef COSTFUNCTION_H
 2 | #define COSTFUNCTION_H
 3 | 
 4 | #include "utility.h"
 5 | 
 6 | #define MAX_VALID_ERROR 1e14
 7 | #define SCALE_FACTOR 2
 8 | 
 9 | using namespace Eigen;
10 | 
11 | class LossFunction
12 | {
13 | public:
14 |     LossFunction() : a_(1.0), b_(1.0) {}
15 |     LossFunction(DT a) : a_(a), b_(a * a) {}
16 |     virtual ~LossFunction() {}
17 | public:
18 |     virtual DT Loss(DT) const = 0;
19 |     virtual DT FirstOrderDerivative(DT error) const = 0;
20 |     virtual DT SecondOrderDerivative(DT error) const = 0;
21 | 
22 | public:
23 |     void CorrectResiduals(Vec2 & residual) const;
24 |     template<size_t R, size_t C>
25 |     void CorrectJacobian(Vec2 const & residual,
26 |                          Matrix<DT, R, C, RowMajor> & jacobian) const;
27 | 
28 | protected:
29 |     const DT a_;
30 |     const DT b_;
31 | };
32 | 
33 | class HuberLoss : public LossFunction
34 | {
35 | public:
36 |     HuberLoss() : LossFunction(SCALE_FACTOR) {}
37 |     HuberLoss(DT a) : LossFunction(a) {}
38 | 
39 | public:
40 |     DT Loss(DT error) const;
41 |     DT FirstOrderDerivative(DT error) const;
42 |     DT SecondOrderDerivative(DT error) const;
43 | };
44 | 
45 | class CauchyLoss : public LossFunction
46 | {
47 | public:
48 |     CauchyLoss() : LossFunction(SCALE_FACTOR) {}
49 |     CauchyLoss(DT a) : LossFunction(a) {}
50 | 
51 | public:
52 |     DT Loss(DT error) const;
53 |     DT FirstOrderDerivative(DT error) const;
54 |     DT SecondOrderDerivative(DT error) const;
55 | };
56 | 
57 | class NULLLoss : public LossFunction
58 | {
59 | public:
60 |     NULLLoss() {}
61 | 
62 | public:
63 |     DT Loss(DT error) const { return error > MAX_VALID_ERROR ? 0.0 : error; }
64 |     DT FirstOrderDerivative(DT error) const { return 1.0; }
65 |     DT SecondOrderDerivative(DT error) const { return 0.0; }
66 | };
67 | 
68 | enum LossType
69 | {
70 |     NULLLossType = 0,
71 |     HuberLossType = 1,
72 |     CauchyLossType = 2
73 | };
74 | 
75 | #include "lossfunction.tpp"
76 | 
77 | #endif // COSTFUNCTION_H
78 | 


--------------------------------------------------------------------------------
/src/lossfunction.cpp:
--------------------------------------------------------------------------------
 1 | #include "STBA/lossfunction.h"
 2 | 
 3 | #include <iostream>
 4 | 
 5 | /*!
 6 |  * @brief Please read corrector.h/cc for reference
 7 |  */
 8 | void LossFunction::CorrectResiduals(Vec2 & residual) const
 9 | {
10 |     DT sq_norm = residual.squaredNorm();
11 |     DT rho1 = FirstOrderDerivative(sq_norm);
12 |     DT rho2 = SecondOrderDerivative(sq_norm);
13 |     DT sqrt_rho1 = std::sqrt(rho1);
14 | 
15 |     DT residual_scaling, alpha_sq_norm;
16 |     if (sq_norm > MAX_VALID_ERROR)
17 |     {
18 |         residual_scaling = 0.0;
19 |     }
20 |     else if ((sq_norm == 0.0) || (rho2 <= 0.0))
21 |     {
22 |         residual_scaling = sqrt_rho1;
23 |         alpha_sq_norm = 0.0;
24 |     }
25 |     else
26 |     {
27 |         assert(rho1 > 0.0 && "[CorrectResiduals] First order derivative mush be positive");
28 |         DT D = 1.0 + 2.0 * sq_norm * rho2 / rho1;
29 |         DT alpha = 1.0 - std::sqrt(D);
30 |         residual_scaling = sqrt_rho1 / (1.0 - alpha);
31 |         alpha_sq_norm = alpha / sq_norm;
32 |     }
33 |     residual *= residual_scaling;
34 | }
35 | 
36 | 
37 | /*!
38 |  * @param error - the inrobust error term. For least square problems, it is the square of residual.
39 |  */
40 | DT HuberLoss::Loss(DT error) const
41 | {
42 |     assert(error >= 0 && "[Loss] Error term must be nonnegative");
43 |     if (error > MAX_VALID_ERROR)
44 |         return 0.0;
45 |     else if (error > b_)
46 |     {
47 |         DT r = std::sqrt(error);
48 |         return 2 * a_ * r - b_;
49 |     }
50 |     return error;
51 | }
52 | 
53 | DT HuberLoss::FirstOrderDerivative(DT error) const
54 | {
55 |     assert(error >= 0 && "[FirstOrderDerivative] Error term must be nonnegative");
56 |     if (error > b_)
57 |     {
58 |         DT r = std::sqrt(error);
59 |         return a_ / r;
60 |     }
61 |     return 1.0;
62 | }
63 | 
64 | DT HuberLoss::SecondOrderDerivative(DT error) const
65 | {
66 |     assert(error >= 0 && "[FirstOrderDerivative] Error term must be nonnegative");
67 |     if (error > b_)
68 |     {
69 |         DT r = std::sqrt(error);
70 |         return -a_ / (2 * error * r);
71 |     }
72 |     return 0.0;
73 | }
74 | 
75 | DT CauchyLoss::Loss(DT error) const
76 | {
77 |     assert(error >= 0 && "[Loss] Error term must be nonnegative");
78 |     if (error > MAX_VALID_ERROR)
79 |         return 0.0;
80 |     return b_ * log(1 + error / b_);
81 | }
82 | 
83 | DT CauchyLoss::FirstOrderDerivative(DT error) const
84 | {
85 |     assert(error >= 0 && "[FirstOrderDerivative] Error term must be nonnegative");
86 |     return b_ / (b_ + error);
87 | }
88 | 
89 | DT CauchyLoss::SecondOrderDerivative(DT error) const
90 | {
91 |     assert(error >= 0 && "[FirstOrderDerivative] Error term must be nonnegative");
92 |     return -b_ / ((b_ + error) * (b_ + error));
93 | }
94 | 
95 | 
96 | 


--------------------------------------------------------------------------------
/src/test.cpp:
--------------------------------------------------------------------------------
 1 | #include "STBA/test.h"
 2 | #include "STBA/lmbaproblem.h"
 3 | 
 4 | 
 5 | void SynthesizeBundleBlock(BundleBlock & bundle_block)
 6 | {
 7 |     double focal = 1000;
 8 |     double u = 2000;
 9 |     double v = 1500;
10 |     Vec3 distortion = {0.0, 0.0, 0.0};
11 |     Vec3 aa = {0.0001, 0.0001, 0.0001};
12 |     size_t group_id = 0;
13 |     Vec6 intrinsic;
14 |     intrinsic << focal, u, v, distortion[0], distortion[1], distortion[2];
15 |     BundleBlock::DGroup group(group_id, intrinsic);
16 |     bundle_block.InsertGroup(group);
17 |     const size_t camera_num = 3;
18 |     const size_t track_num = 10;
19 | 
20 |     for (size_t i = 0; i < camera_num; i++)
21 |     {
22 |         Vec3 trans = {i * 1.0, 0.0, 0.0};
23 |         BundleBlock::DCamera camera(i, group_id, aa, trans);
24 |         bundle_block.InsertCamera(camera);
25 |     }
26 | 
27 |     size_t proj_id = 0;
28 |     for (size_t i = 0; i < track_num; i++)
29 |     {
30 |         Vec3 point = {i * 0.2, 0.0, 10.0};
31 |         BundleBlock::DTrack track(i, point);
32 |         bundle_block.InsertTrack(track);
33 | 
34 |         for (size_t j = 0; j < 3; j++)
35 |         {
36 |             BundleBlock::DCamera const & camera = bundle_block.GetCamera(j);
37 |             Vec2 proj;
38 |             Project(focal, u, v, camera.axis_angle, camera.translation, point, distortion, proj);
39 |             proj[0] += 5.0;
40 |             proj[1] += 5.0;
41 |             BundleBlock::DProjection projection(proj_id++, j, i, proj);
42 |             bundle_block.InsertProjection(projection);
43 |         }
44 |     }
45 |     for (size_t i = 0; i < camera_num; i++)
46 |     {
47 |         BundleBlock::DCamera & camera = bundle_block.GetCamera(i);
48 |         for (size_t j = 0; j < camera_num; j++)
49 |         {
50 |             if (i != j)
51 |             {
52 |                 camera.linked_cameras.insert(j);
53 |             }
54 |         }
55 |     }
56 | }
57 | 
58 | void TestLM()
59 | {
60 |     BundleBlock bundle_block;
61 |     SynthesizeBundleBlock(bundle_block);
62 | 
63 |     LMBAProblem problem(1, 1e2, static_cast<LossType>(0));
64 |     problem.Initialize(bundle_block);
65 |     problem.Solve();
66 |     problem.Update(bundle_block);
67 | 
68 |     std::vector<size_t> group_indexes = bundle_block.GroupIndexes();
69 |     std::vector<size_t> camera_indexes = bundle_block.CameraIndexes();
70 |     std::vector<size_t> track_indexes = bundle_block.TrackIndexes();
71 | 
72 |     for (size_t i = 0; i < group_indexes.size(); i++)
73 |     {
74 |         bundle_block.GetGroup(group_indexes[i]).Print();
75 |     }
76 |     for (size_t i = 0; i < camera_indexes.size(); i++)
77 |     {
78 |         bundle_block.GetCamera(camera_indexes[i]).Print();
79 |     }
80 |     for (size_t i = 0; i < track_indexes.size(); i++)
81 |     {
82 |         bundle_block.GetTrack(track_indexes[i]).Print();
83 |     }
84 | }
85 | 


--------------------------------------------------------------------------------
/include/STBA/clustering/louvain.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOUVAIN_H
 2 | #define LOUVAIN_H
 3 | 
 4 | #include <unordered_map>
 5 | #include "STBA/clustering/graph.h"
 6 | 
 7 | class Louvain
 8 | {
 9 | public:
10 |     Louvain();
11 | 
12 |     Louvain(std::vector<size_t> const & nodes,
13 |             std::unordered_map<size_t, std::unordered_map<size_t, double> > const & edges);
14 | 
15 |     Louvain(std::vector<size_t> const & nodes,
16 |             std::vector<std::pair<size_t, size_t> > const & edges);
17 | 
18 |     ~Louvain();
19 | 
20 |     void Cluster();
21 |     void Cluster(std::vector<std::pair<size_t, size_t> > const & initial_pairs);
22 |     void StochasticCluster();
23 |     void StochasticCluster(std::vector<std::pair<size_t, size_t> > const & initial_pairs);
24 | 
25 |     void MCMCSampling();
26 | 
27 |     void Initialize(std::vector<size_t> const & nodes,
28 |                     std::vector<std::pair<size_t, size_t> > const & edges);
29 |     void Initialize(std::vector<size_t> const & nodes,
30 |                     std::unordered_map<size_t, std::unordered_map<size_t, double> > const & edges);
31 |     void Reinitialize();
32 | 
33 |     void Print();
34 | 
35 |     double Modularity(size_t const community) const;
36 |     double Modularity() const;
37 | 
38 |     size_t NodeNum() const { return node_graph_.NodeNum(); }
39 |     size_t EdgeNum() const { return node_graph_.EdgeNum(); }
40 | 
41 |     inline double SumEdgeWeight() const { return sum_edge_weight_; }
42 |     double EdgeWeight(size_t const, size_t const) const;
43 | 
44 |     void SetMaxCommunity(size_t s) { max_community_ = s; }
45 |     size_t GetMaxCommunity() const { return max_community_; }
46 |     void SetTemperature(double t) { temperature_ = t; }
47 |     double GetTemperature() const { return temperature_; }
48 | 
49 |     void GetClusters(std::vector<std::vector<size_t> > &) const;
50 |     void GetEdgesAcrossClusters(std::vector<std::pair<size_t, size_t> > & pairs) const;
51 | 
52 | private:
53 |     void RearrangeCommunities();
54 |     bool Merge();
55 |     bool Merge(std::vector<std::pair<size_t, size_t> > const & initial_pairs);
56 |     bool StochasticMerge();
57 |     void Rebuild();
58 | 
59 | private:
60 |     Graph graph_;
61 |     Graph node_graph_;
62 |     std::vector<int> node_map_;                                 // node index to community index
63 |     std::vector<int> community_map_;                        // prev community index to community index
64 |     std::vector<size_t> community_size_;                    // size of community
65 |     std::vector<double> community_in_weight_;          // sum of edge weights inside a community
66 |     std::vector<double> community_total_weight_;      // sum of edge weights incident to nodes of a community, including those inside a community
67 |     double sum_edge_weight_;    // 2m
68 |     size_t max_community_;
69 |     double temperature_;
70 | 
71 | };
72 | 
73 | void TestMCMCSampling();
74 | void Test();
75 | 
76 | #endif // LOUVAIN_H
77 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # STBA
 2 | This is a C++ implementation of our ECCV 2020 paper - ["Stochastic Bundle Adjustment for Efficient and Scalable 3D Reconstruction"](https://arxiv.org/abs/2008.00446) by Lei Zhou, Zixin Luo, Mingmin Zhen, Tianwei Shen, Shiwei Li, Zhuofei Huang, Tian Fang, Long Quan.
 3 | 
 4 | If you find this project useful, please cite:
 5 |    
 6 | ```
 7 | @inproceedings{zhou2020kfnet,
 8 |   title={Stochastic Bundle Adjustment for Efficient and Scalable 3D Reconstruction},
 9 |   author={Zhou, Lei and Luo, Zixin and Zhen, Mingmin and Shen, Tianwei and Li, Shiwei and Huang, Zhuofei and Fang, Tian and Quan, Long},
10 |   booktitle={European Conference on Computer Vision (ECCV)},
11 |   year={2020}
12 | }
13 | ```
14 | 
15 | ## About
16 | 
17 | ### Introduction
18 | 
19 | One of the most expensive steps of bundle adjustment is to solve the **Reduced Camera System (RCS)** in each iteration whose dimension is proportional to the camera number. In this work, we propose **ST**ochastic **B**undle **A**djustment (STBA) to decompose the RCS into indepedent subproblems in a stochastic way to improve the efficiency and scalability.
20 | 
21 | 
22 | ### Benchmark on [1DSfM dataset](http://www.cs.cornell.edu/projects/1dsfm/)
23 | 
24 | Below we show the convergence curves of STBA on the SfM dataset in comparison with:
25 | 	
26 | * ```LM-sparse```: Levenberg-Marquardt algorithm with sparse linear solver,
27 | * ```LM-iterative```: Levenberg-Marquardt algorithm with conjugate gradient solver,
28 | * ```DL```: Dogleg algorithm.
29 | 	
30 | ![1DSfM](doc/1DSfM.jpg)
31 | 
32 | 
33 | ### Benchmark on [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php)
34 | 
35 | Below we show the convergence curves of STBA on the SLAM dataset.
36 | 
37 | ![KITTI](doc/KITTI.jpg)
38 | 
39 | 
40 | ### Benchmark on Large-Scale dataset
41 | 
42 | Lastly we show the distributed experiments on the city-scale 3D dataset collected by ourselves. Each data has around 30,000 images and can not be processed by a single compute node. We compare the distributed implementation of STBA against the state-of-the-art distributed bundle adjustment solver [DBACC](https://zlthinker.github.io/files/distributed_bundle.pdf).
43 | 
44 | ![largs_scale](doc/large_scale.jpg)
45 | 
46 | 
47 | 
48 | ## Usage
49 | 
50 | ### Requirements
51 | 
52 | * Eigen3
53 | * OpenMP 
54 | 
55 | ### Code version
56 | 
57 | This repository currently comprises two versions of codes ```0.0.0``` and ```1.0.0``` which go into the ```master``` and ```1.0.0``` branches, respectively.
58 | The ```0.0.0``` codes are based on the plain workflow of an Levenberg-Marquardt solver, while the ```1.0.0``` codes are based on the implementation suggested by [[1] Bundle Adjustment Rules](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.222.3253&rep=rep1&type=pdf) and are supposed to be more computation and memory efficient. You can try different implementations by checking out different branches.
59 | 
60 | ### Build
61 |  
62 |  ```
63 |  cd STBA
64 |  mkdir build && cd build
65 |  cmake ..
66 |  make
67 |  ```
68 | 
69 | ### Run
70 | 
71 | * Download the sparse reconstruction results of the [COLMAP dataset](https://colmap.github.io/datasets.html) (e.g., Gerrard Hall).
72 | * Unzip the compressed file and three files can be found in the `sparse` folder: `cameras.txt`, `images.txt` and `points3D.txt`.
73 | * ``` cd STBA/build ```
74 | * ``` ./STBA <path_to_cameras.txt> <path_to_imagess.txt>  <path_to_points3D.txt> <output_folder>```
75 | * Run ```./STBA --help``` to see more options.
76 | 	* `--iteration`: Set the maximum number of iterations.
77 | 	* `--cluster`: A STBA option which sets the maximum cluster size for stochastic graph clustering.
78 | 	* `--inner_step`: A STBA option which sets the number of inner iterative steps.
79 | 	* `--thread_num`: Set thread number for OpenMP parallel computing.
80 | 	* `--radius`: Set the initial radius of trust region for the trust region solvers.
81 | 	* `--loss`: Set the type of robust loss function. Currently, `Huber` and `Cauchy` loss functions are supported.
82 | 	* `--noise`: The magnitude/sigma of random Gaussian noise which will be added to track positions and camera centers for testing the bundle adjustment algorithms.
83 | 	* `--lm`: Use Levenberg-Marquardt solver.
84 | 	* `--dl`: Use DogLeg solver.
85 | 
86 | ### TODO
87 | 
88 | * Add parallel support to version ```1.0.0```, particularly the function ```StochasticBAProblem::EvaluateCamera()```.
89 | 
90 | 
91 | 
92 | ## Credit
93 | 
94 | This implementation was developed by [Lei Zhou](https://zlthinker.github.io/). Feel free to contact Lei for any enquiry.
95 | 
96 | ## Reference
97 | 
98 | [1] Engels, Chris, Henrik Stewénius, and David Nistér. "Bundle Adjustment Rules." Photogrammetric computer vision, 2(2006).
99 | 


--------------------------------------------------------------------------------
/include/STBA/stochasticbaproblem.h:
--------------------------------------------------------------------------------
  1 | #ifndef STOCHASTICBAPROBLEM_H
  2 | #define STOCHASTICBAPROBLEM_H
  3 | 
  4 | #include "lmbaproblem.h"
  5 | #include "clustering/louvain.h"
  6 | 
  7 | class StochasticBAProblem : public LMBAProblem
  8 | {
  9 | public:
 10 |     StochasticBAProblem();
 11 |     StochasticBAProblem(size_t max_iter,
 12 |                         double radius,
 13 |                         LossType loss_type,
 14 |                         size_t max_community,
 15 |                         size_t inner_step);
 16 |     StochasticBAProblem(size_t max_iter,
 17 |                         double radius,
 18 |                         LossType loss_type,
 19 |                         size_t max_community,
 20 |                         double temperature,
 21 |                         size_t batch_size,
 22 |                         size_t inner_step,
 23 |                         bool complementary_clustering);
 24 |     StochasticBAProblem(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num);
 25 |     virtual ~StochasticBAProblem();
 26 | 
 27 |     inline void SetMaxCommunity(size_t val) { cluster_->SetMaxCommunity(val); }
 28 |     inline size_t GetMaxCommunity() const { return cluster_->GetMaxCommunity(); }
 29 |     inline void SetBatchSize(size_t val)    { batch_size_ = val; }
 30 |     inline void SetInnerStep(size_t val)    { inner_step_ = val; }
 31 |     inline void SetComplementaryClustering(bool val)    { complementary_clustering_ = val; }
 32 | 
 33 |     virtual void Solve();
 34 |     double EvaluateRSquare(VecX const & aug_pose_diagonal,
 35 |                            std::vector<std::vector<Vec3> > const & aug_point_diagonal) const;
 36 |     double EvaluateRSquare2(VecX const & aug_diagonal);
 37 |     bool StepAccept();
 38 |     virtual bool Initialize(BundleBlock const & bundle_block);
 39 |     void SaveCameraCluster(std::string const & save_path);
 40 | 
 41 | protected:
 42 |     size_t GetPoseCluster(size_t) const;
 43 |     size_t GetPointLocalCluster(size_t, size_t) const;
 44 |     size_t GetPointClusters(size_t, std::vector<size_t> &) const;
 45 |     void GetPointDiagonal(std::vector<std::vector<Vec3> > &) const;
 46 |     void GetPointAugDiagonal(std::vector<std::vector<Vec3> > &) const;
 47 |     void AddPointDiagonal(std::vector<std::vector<Vec3> > const &);
 48 |     void SetPointDiagonal(std::vector<std::vector<Vec3> > const &);
 49 |     void EvaluateJpJp(size_t point_index, size_t cluster_index, Mat3 & JpJp) const;
 50 |     virtual void EvaluateJpJp();
 51 |     void EvaluateJpe(size_t point_index, size_t cluster_index, Vec3 & Jpe) const;
 52 |     virtual void EvaluateJpe();
 53 |     bool EvaluateDeltaPose(std::vector<size_t> const & pose_indexes, VecX const & b, VecX & dy) const;
 54 |     virtual bool EvaluateDeltaPose();
 55 |     virtual void EvaluateDeltaPoint(size_t point_index, Vec3 & dz);
 56 |     virtual void EvaluateDeltaPoint();
 57 |     virtual void EvaluateEcw(size_t pose_index, Vec6 & Ecw) const;
 58 |     void EvaluateEDeltaPose(size_t point_index, size_t cluster_index, Vec3 & Edy) const;
 59 |     virtual bool EvaluateEcEc(size_t pose_index1, size_t pose_index2, Mat6 & EcEc) const;
 60 |     virtual void EvaluateEcEc(std::vector<size_t> const & pose_indexes, MatX & EcEc) const;
 61 |     void EvaluateSchurComplement(std::vector<std::unordered_map<size_t, Mat6> > & S) const;
 62 |     void EvaluateFullb(VecX & b) const;
 63 |     void EvaluateSdy(std::vector<std::unordered_map<size_t, Mat6> > const & S, VecX const & dy, VecX & Sdy) const;
 64 |     virtual void AugmentPointDiagonal();
 65 |     virtual void ResetPointDiagonal();
 66 |     void GetJpJp(size_t point_index, size_t cluster_index, Mat3 & JpJp) const;
 67 |     void SetJpJp(size_t point_index, size_t cluster_index, Mat3 const & JpJp);
 68 |     void GetJpe(size_t point_index, size_t cluster_index, Vec3 & Jpe) const;
 69 |     void SetJpe(size_t point_index, size_t cluster_index, Vec3 const & Jpe);
 70 |     void GetDeltaPoint(size_t point_index, size_t cluster_index, Vec3 & dz) const;
 71 |     void SetDeltaPoint(size_t point_index, size_t cluster_index, Vec3 const & dz);
 72 |     void ClearPointMeta();
 73 |     void InnerIteration(VecX & dy) const;
 74 |     void SteepestDescentCorrection(size_t const point_index);
 75 |     void SteepestDescentCorrection();
 76 | 
 77 | private:
 78 |     void InitializeCluster();
 79 |     void RunCluster();
 80 |     virtual void Print();
 81 | 
 82 | private:
 83 |     Louvain * cluster_;
 84 |     std::vector<std::vector<size_t> > cluster_poses_;
 85 |     std::vector<std::vector<size_t> > cluster_points_;
 86 |     std::vector<size_t> pose_cluster_map_;
 87 |     std::vector<std::vector<size_t> > point_cluster_map_;
 88 |     std::vector<PointMeta* > point_meta_;
 89 |     double last_square_residual_;
 90 |     double square_residual_;
 91 |     double R_square_;
 92 |     double phi_;
 93 |     size_t batch_size_;
 94 |     double connectivity_sample_ratio_;   // sample ratio of camera connectivity
 95 |     std::vector<std::vector<Vec3> > cluster_point_diagonal_;
 96 |     // for inner iterations
 97 |     size_t inner_step_;
 98 |     std::vector<std::unordered_map<size_t, Mat6> > full_S_;
 99 |     VecX full_b_;
100 |     bool complementary_clustering_;
101 | };
102 | 
103 | #endif // STOCHASTICBAPROBLEM_H
104 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/clustering/louvain.h"
  2 | #include "STBA/stochasticbaproblem.h"
  3 | #include "STBA/lmbaproblem.h"
  4 | #include "STBA/dlbaproblem.h"
  5 | #include "STBA/test.h"
  6 | 
  7 | void PrintHelp()
  8 | {
  9 |     std::cout << "<exe> <cameras.txt> <images.txt> <points.txt> <output_folder>\n"
 10 |               << "--iteration <val> : Set maximum iteration, default val = 100 \n"
 11 |               << "--cluster <val> : Set maximum cluster size, default val = 100 \n"
 12 |               << "--inner_step <val> : Set number of correction steps, default val = 4 \n"
 13 |               << "--thread_num <val> : Set thread number, default val = 1 \n"
 14 |               << "--radius <val> : Set intial radius of trust region, default val = 10000 \n"
 15 |               << "--loss <val> : Set loss type (0 - NULL, 1 - Huber, 2 - Cauchy), default val = 2 \n"
 16 |               << "--noise <val> : Set sigma of Gaussian noise, default val = 0.0 \n"
 17 |               << "--lm : Use Levenberg Marquardt \n"
 18 |               << "--dl : Use DogLeg \n";
 19 | }
 20 | 
 21 | int main(int argc, char **argv)
 22 | {
 23 |     std::cout << "STBA version: " << STBA_VERSION << std::endl;
 24 |     if (argc < 5)
 25 |     {
 26 |         PrintHelp();
 27 |         return -1;
 28 |     }
 29 |     std::string cameras_path = argv[1];
 30 |     std::string images_path = argv[2];
 31 |     std::string points_path = argv[3];
 32 |     std::string output_folder = argv[4];
 33 |     size_t iteration = 100;
 34 |     size_t cluster = 100;
 35 |     size_t inner_step = 4;
 36 |     size_t thread_num = 1;
 37 |     double radius = 10000;
 38 |     LossType loss_type = CauchyLossType;
 39 |     double noise = 0.0;
 40 |     bool lm = false;
 41 |     bool dl = false;
 42 | 
 43 |     size_t i = 5;
 44 |     while (i < argc)
 45 |     {
 46 |         std::string option = argv[i];
 47 |         if (option == "--iteration")
 48 |         {
 49 |             i++;
 50 |             if (i >= argc)
 51 |             {
 52 |                 PrintHelp();
 53 |                 return -1;
 54 |             }
 55 |             iteration = static_cast<size_t>(std::stoi(argv[i]));
 56 |         }
 57 |         else if (option == "--cluster")
 58 |         {
 59 |             i++;
 60 |             if (i >= argc)
 61 |             {
 62 |                 PrintHelp();
 63 |                 return -1;
 64 |             }
 65 |             cluster = static_cast<size_t>(std::stoi(argv[i]));
 66 |         }
 67 |         else if (option == "--inner_step")
 68 |         {
 69 |             i++;
 70 |             if (i >= argc)
 71 |             {
 72 |                 PrintHelp();
 73 |                 return -1;
 74 |             }
 75 |             inner_step = static_cast<size_t>(std::stoi(argv[i]));
 76 |         }
 77 |         else if (option == "--thread_num")
 78 |         {
 79 |             i++;
 80 |             if (i >= argc)
 81 |             {
 82 |                 PrintHelp();
 83 |                 return -1;
 84 |             }
 85 |             thread_num = static_cast<size_t>(std::stoi(argv[i]));
 86 |         }
 87 |         else if (option == "--radius")
 88 |         {
 89 |             i++;
 90 |             if (i >= argc)
 91 |             {
 92 |                 PrintHelp();
 93 |                 return -1;
 94 |             }
 95 |             radius = static_cast<double>(std::stod(argv[i]));
 96 |         }
 97 |         else if (option == "--loss")
 98 |         {
 99 |             i++;
100 |             if (i >= argc)
101 |             {
102 |                 PrintHelp();
103 |                 return -1;
104 |             }
105 |             if (loss_type != 0 && loss_type != 1 && loss_type != 2)
106 |             {
107 |                 std::cout << "Invalid loss type: " << loss_type << "\n";
108 |                 PrintHelp();
109 |                 return -1;
110 |             }
111 |             loss_type = static_cast<LossType>(std::stoi(argv[i]));
112 |         }
113 |         else if (option == "--noise")
114 |         {
115 |             i++;
116 |             if (i >= argc)
117 |             {
118 |                 PrintHelp();
119 |                 return -1;
120 |             }
121 |             noise = std::abs(static_cast<double>(std::stod(argv[i])));
122 |         }
123 |         else if (option == "--lm")
124 |         {
125 |             lm = true;
126 |         }
127 |         else if (option == "--dl")
128 |         {
129 |             dl = true;
130 |         }
131 |         else if (option == "--help" || option == "--h")
132 |         {
133 |             PrintHelp();
134 |             return 0;
135 |         }
136 |         else
137 |         {
138 |             std::cout << "Invalid option: " << option << "\n";
139 |             PrintHelp();
140 |             return -1;
141 |         }
142 |         i++;
143 |     }
144 | 
145 | 
146 |     std::cout << cameras_path << "\n"
147 |               << images_path << "\n"
148 |               << points_path << "\n"
149 |               << output_folder << "\n"
150 |               << "iteration = " << iteration << "\n"
151 |               << "cluster = " << cluster << "\n"
152 |               << "inner_step = " << inner_step << "\n"
153 |               << "thread_num = " << thread_num << "\n"
154 |               << "radius = " << radius << "\n"
155 |               << "loss_type = " << loss_type << "\n"
156 |               << "noise = " << noise << "\n"
157 |               << "lm = " << lm << "\n"
158 |               << "dl = " << dl << "\n";
159 | 
160 |     BundleBlock bundle_block;
161 |     bundle_block.LoadColmapTxt(cameras_path, images_path, points_path);
162 |     if (noise > 0)
163 |     {
164 |         bundle_block.AddGaussianNoiseToTrack(0, noise);
165 |         bundle_block.AddGaussianNoiseToCameraTranslation(0, noise);
166 |     }
167 | 
168 |     BAProblem * problem;
169 |     if (lm)
170 |     {
171 |         problem = new LMBAProblem(iteration, radius, loss_type);
172 |     }
173 |     else if (dl)
174 |     {
175 |         problem = new DLBAProblem(iteration, radius, loss_type);
176 |     }
177 |     else
178 |     {
179 |         problem = new StochasticBAProblem(iteration, radius, loss_type, cluster, inner_step);
180 |     }
181 |     problem->SetThreadNum(thread_num);
182 | 
183 |     if (!problem->Initialize(bundle_block))
184 |     {
185 |         std::cout << "Fail to initialize bundle problem.\n";
186 |         delete problem;
187 |         return -1;
188 |     }
189 |     problem->Solve();
190 |     problem->Update(bundle_block);
191 |     problem->SaveReport(JoinPath(output_folder, "report.txt"));
192 |     delete problem;
193 | 
194 |     bundle_block.SaveColmapTxt(JoinPath(output_folder, "cameras.txt"), JoinPath(output_folder, "images.txt"), JoinPath(output_folder, "points.txt"));
195 | 
196 |     return 0;
197 | }
198 | 


--------------------------------------------------------------------------------
/include/STBA/utility.h:
--------------------------------------------------------------------------------
  1 | #ifndef UTILITY_H
  2 | #define UTILITY_H
  3 | 
  4 | #include <Eigen/Dense>
  5 | #include <Eigen/Sparse>
  6 | #include <unordered_map>
  7 | #include <numeric>
  8 | 
  9 | typedef double DT;
 10 | #define EPSILON double(1e-12)
 11 | #define MIN_DEPTH double(1e-12)
 12 | #define MAX_REPROJ_ERROR double(1e6)
 13 | 
 14 | using namespace Eigen;
 15 | 
 16 | typedef Eigen::Matrix<DT, 2, 1, Eigen::ColMajor> Vec2;
 17 | typedef Eigen::Matrix<DT, 3, 1, Eigen::ColMajor> Vec3;
 18 | typedef Eigen::Matrix<DT, 4, 1, Eigen::ColMajor> Vec4;
 19 | typedef Eigen::Matrix<DT, 6, 1, Eigen::ColMajor> Vec6;
 20 | typedef Eigen::Matrix<DT, Eigen::Dynamic, 1> VecX;
 21 | typedef Eigen::Matrix<DT, 2, 3, Eigen::RowMajor> Mat23;
 22 | typedef Eigen::Matrix<DT, 2, 6, Eigen::RowMajor> Mat26;
 23 | typedef Eigen::Matrix<DT, 2, 2, Eigen::RowMajor> Mat2;
 24 | typedef Eigen::Matrix<DT, 3, 3, Eigen::RowMajor> Mat3;
 25 | typedef Eigen::Matrix<DT, 6, 6, Eigen::RowMajor> Mat6;
 26 | typedef Eigen::Matrix<DT, 6, 3, Eigen::RowMajor> Mat63;
 27 | typedef Eigen::Matrix<DT, 9, 3, Eigen::RowMajor> Mat93;
 28 | typedef Eigen::Matrix<DT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatX;
 29 | typedef Eigen::SparseMatrix<DT> SMat;
 30 | typedef AngleAxis<DT> AxisAngle;
 31 | 
 32 | DT Determinant(Mat3 const & M);
 33 | Mat3 AngleAxis2Matrix(Vec3 const & angle_axis);
 34 | Vec3 Quaternion2AngleAxis(Vec4 const & quaternion);
 35 | Vec4 AngleAxis2Quaternion(Vec3 const & angle_axis);
 36 | 
 37 | Vec3 RotatePoint(Vec3 const & angle_axis, Vec3 const & point);
 38 | 
 39 | Mat26 Projection2IntrinsicJacobian(double const focal,
 40 |                                    Vec3 const & radial_distortion,
 41 |                                    Vec3 const & angle_axis,
 42 |                                    Vec3 const & translation,
 43 |                                    Vec3 const & global_point);
 44 | 
 45 | Mat23 Projection2RotationJacobian(double const focal,
 46 |                                   Vec3 const & radial_distortion,
 47 |                                   Vec3 const & angle_axis,
 48 |                                   Vec3 const & translation,
 49 |                                   Vec3 const & global_point);
 50 | 
 51 | Mat23 Projection2TranslationJacobian(double const focal,
 52 |                                      Vec3 const & radial_distortion,
 53 |                                      Vec3 const & angle_axis,
 54 |                                      Vec3 const & translation,
 55 |                                      Vec3 const & global_point);
 56 | 
 57 | Mat23 Projection2GlobalPointJacobian(double const focal,
 58 |                                      Vec3 const & radial_distortion,
 59 |                                      Vec3 const & angle_axis,
 60 |                                      Vec3 const & translation,
 61 |                                      Vec3 const & global_point);
 62 | 
 63 | void ProjectAndGradient(Vec3 const & rotation, Vec3 const & translation, Vec3 const & point,
 64 |                         double const focal, double const u, double const v,
 65 |                         Vec3 const & radial_distortion, Vec2 const & projection,
 66 |                         Mat23 & rotation_jacobian,
 67 |                         Mat23 & translation_jacobian,
 68 |                         Mat23 & point_jacobian,
 69 |                         Mat26 & intrinsic_jacobian);
 70 | 
 71 | bool Project(double const focal, double const u, double const v,
 72 |              Vec3 const & angle_axis, Vec3 const & translation, Vec3 const & global_point,
 73 |              Vec3 const & radial_distortion,
 74 |              Vec2 & reprojection);
 75 | 
 76 | double RandomNoise(double min, double max);
 77 | 
 78 | template <class T>
 79 | T const & RandomNoise(T & data, double min, double max)
 80 | {
 81 |     for (int i = 0; i < data.size(); i++)
 82 |     {
 83 |         *(data.data() + i) += DT(RandomNoise(min, max));
 84 |     }
 85 |     return data;
 86 | }
 87 | 
 88 | 
 89 | double GaussianNoise(double mean, double sigma);
 90 | 
 91 | template <class T>
 92 | bool IsNumericalValid(T const & data)
 93 | {
 94 |     for (int i = 0; i < data.size(); i++)
 95 |     {
 96 |         DT val = *(data.data() + i);
 97 |         if (std::isnan(val) || std::isinf(val))
 98 |             return false;
 99 |     }
100 |     return true;
101 | }
102 | 
103 | struct UnionFind
104 | {
105 |     // Represent the DS/UF forest thanks to two array:
106 |     // A parent 'pointer tree' where each node holds a reference to its parent node
107 |     std::vector<unsigned int> m_cc_parent;
108 |     // A rank array used for union by rank
109 |     std::vector<unsigned int> m_cc_rank;
110 |     // A 'size array' to know the size of each connected component
111 |     std::vector<unsigned int> m_cc_size;
112 | 
113 |     // Init the UF structure with num_cc nodes
114 |     void InitSets(const unsigned int num_cc)
115 |     {
116 |         // all set size are 1 (independent nodes)
117 |         m_cc_size.resize(num_cc, 1);
118 |         // Parents id have their own CC id {0,n}
119 |         m_cc_parent.resize(num_cc);
120 |         for(size_t i = 0; i < num_cc; i++)
121 |         {
122 |             m_cc_parent[i] = i;
123 |         }
124 | 
125 |         // Rank array (0)
126 |         m_cc_rank.resize(num_cc, 0);
127 |     }
128 | 
129 |     // Return the number of nodes that have been initialized in the UF tree
130 |     unsigned int GetNumNodes() const
131 |     {
132 |         return m_cc_size.size();
133 |     }
134 | 
135 |     // Return the representative set id of I nth component
136 |     unsigned int Find(unsigned int i)
137 |     {
138 |         // Recursively set all branch as children of root (Path compression)
139 |         if (m_cc_parent[i] != i)
140 |             m_cc_parent[i] = Find(m_cc_parent[i]);
141 |         return m_cc_parent[i];
142 |     }
143 | 
144 |     // Replace sets containing I and J with their union
145 |     void Union(unsigned int i, unsigned int j)
146 |     {
147 |         i = Find(i);
148 |         j = Find(j);
149 |         if(i==j)
150 |         {
151 |             // Already in the same set. Nothing to do
152 |             return;
153 |         }
154 | 
155 |         // x and y are not already in same set. Merge them.
156 |         // Perform an union by rank:
157 |         //  - always attach the smaller tree to the root of the larger tree
158 |         if(m_cc_rank[i] < m_cc_rank[j])
159 |         {
160 |             m_cc_parent[i] = j;
161 |             m_cc_size[j] += m_cc_size[i];
162 |         }
163 |         else if(m_cc_rank[i] > m_cc_rank[j])
164 |         {
165 |             m_cc_parent[j] = i;
166 |             m_cc_size[i] += m_cc_size[j];
167 |         }
168 |         else {
169 |             m_cc_parent[j] = i;
170 |             m_cc_size[i] += m_cc_size[j];
171 |             m_cc_rank[i]++;
172 |         }
173 |     }
174 | };
175 | 
176 | template <typename T>
177 | std::vector<size_t> SortIndexes(const std::vector<T> &v, bool increase = true)
178 | {
179 |     // initialize original index locations
180 |     std::vector<size_t> idx(v.size());
181 |     std::iota(idx.begin(), idx.end(), 0);
182 | 
183 |     // sort indexes based on comparing values in v
184 |     if (increase)
185 |         std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) {return v[i1] < v[i2];});
186 |     else
187 |         std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});
188 | 
189 |     return idx;
190 | }
191 | 
192 | bool ReadCameraGroup(std::string const & camera_group_file,
193 |                      std::unordered_map<size_t, size_t> & camera_group_map);
194 | 
195 | bool ReadLinesFromFile(std::string const & file_path, std::vector<std::string> & lines);
196 | 
197 | std::string JoinPath(std::string const & folder, std::string const & file);
198 | 
199 | #endif // UTILITY_H
200 | 


--------------------------------------------------------------------------------
/src/lmbaproblem.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/lmbaproblem.h"
  2 | 
  3 | #include <iostream>
  4 | #include <iomanip>
  5 | 
  6 | LMBAProblem::LMBAProblem() : BAProblem(),
  7 |     max_iteration_(100), mu_(1e4), rho_(0.0), decrease_factor_(3.0)
  8 | {
  9 | 
 10 | }
 11 | 
 12 | LMBAProblem::LMBAProblem(LossType loss_type) : BAProblem(loss_type),
 13 |     max_iteration_(100), mu_(1e4), rho_(0.0), decrease_factor_(3.0)
 14 | {
 15 | 
 16 | }
 17 | 
 18 | LMBAProblem::LMBAProblem(size_t max_iter, double radius, LossType loss_type) : BAProblem(loss_type),
 19 |     max_iteration_(max_iter), mu_(radius), rho_(0.0), decrease_factor_(3.0)
 20 | {
 21 | 
 22 | }
 23 | 
 24 | LMBAProblem::LMBAProblem(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num) :
 25 |     BAProblem(pose_num, group_num, point_num, proj_num),
 26 |     max_iteration_(100), mu_(1e4), rho_(0.0), decrease_factor_(3.0)
 27 | {
 28 | 
 29 | }
 30 | 
 31 | LMBAProblem::~LMBAProblem()
 32 | {
 33 | 
 34 | }
 35 | 
 36 | void LMBAProblem::Solve()
 37 | {
 38 |     last_square_error_ = EvaluateSquareError(false);
 39 |     double mean_error, median_error, max_error;
 40 |     ReprojectionError(mean_error, median_error, max_error, false);
 41 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
 42 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
 43 |     evaluate_ = true;
 44 |     time_ = std::chrono::system_clock::now();
 45 |     for (iter_ = 0; iter_ < max_iteration_; iter_++)
 46 |     {
 47 |         if (evaluate_)
 48 |         {
 49 |             EvaluateResidual();
 50 |             EvaluateJacobian();
 51 |             // evaluate Hessian
 52 |             EvaluateJcJc();
 53 |             EvaluateJcJp();
 54 |             EvaluateJpJp();
 55 |             // evaluate gradient
 56 |             EvaluateJce();
 57 |             EvaluateJpe();
 58 |             ClearUpdate();
 59 |         }
 60 | 
 61 |         // Augment diagonal
 62 |         AugmentPoseDiagonal();
 63 |         AugmentPointDiagonal();
 64 | 
 65 |         EvaluateEcw();
 66 | 
 67 |         // Compute update step
 68 |         if (!EvaluateDeltaPose())
 69 |         {
 70 |             std::cout << "Fail in EvaluateDeltaPose.\n";
 71 |             step_accept_ = false;
 72 |         }
 73 |         else
 74 |         {
 75 |             EvaluateDeltaPoint();               // compute delta point
 76 |             square_error_ = EvaluateSquareError(true);
 77 |             if (StopCriterionGradient() || StopCriterionUpdate()
 78 |                     || StopCriterionRadius() || StopCriterionRelativeCostChange())
 79 |                 break;
 80 |             step_accept_ = StepAccept();
 81 |         }
 82 | 
 83 |         if (step_accept_)                            // accept, descrease lambda
 84 |         {
 85 |             Print();
 86 |             IncreaseRadius();
 87 |             last_square_error_ = square_error_;
 88 |             UpdateParam();
 89 |             evaluate_ = true;                    // Need to re-evaluate once the pose or point parameters are updated
 90 |         }
 91 |         else                                    // reject, increase lambda
 92 |         {
 93 |             Print();
 94 |             ResetPoseDiagonal();
 95 |             ResetPointDiagonal();
 96 |             evaluate_ = false;
 97 |             DecreaseRadius();
 98 |         }
 99 |     }
100 | 
101 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
102 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
103 |     stream_ << "[Solve] Before: mean / median / max reprojection error = "
104 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
105 |     ReprojectionError(mean_error, median_error, max_error, false);
106 |     std::cout << "[Solve] After: mean / median / max reprojection error = "
107 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
108 |     stream_ << "[Solve] After: mean / median / max reprojection error = "
109 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
110 |     stream_ << "[Setting] thread number = " << thread_num_<< "\n";
111 |     stream_ << "[Setting] Levenberg Marquardt\n";
112 | }
113 | 
114 | /*!
115 |  * @brief Decrease step radius (more conservative) when step rejected
116 |  */
117 | void LMBAProblem::DecreaseRadius()
118 | {
119 |     mu_ /= decrease_factor_;
120 | }
121 | 
122 | /*!
123 |  * @brief Increase step radius (more aggressive) when step accepted
124 |  */
125 | void LMBAProblem::IncreaseRadius()
126 | {
127 |     double factor = 1/3.0;
128 |     mu_ = std::min(1e32, mu_ / factor);
129 |     decrease_factor_ = 3.0;
130 | }
131 | 
132 | /*!
133 |  * @brief Evaluate the step quality by computing the ratio of exact cost decrease w.r.t the expected decrease of the linear model.
134 |  * @param aug_diagonal - The incremental quantity added to the diagonal of J^TJ
135 |  */
136 | void LMBAProblem::EvaluateRho(VecX const & aug_diagonal)
137 | {
138 |     double last_error = EvaluateSquareError(false);
139 |     double error = EvaluateSquareError(true);
140 |     double change = last_error - error;
141 | 
142 |     VecX delta_pose, delta_point;
143 |     GetPoseUpdate(delta_pose);
144 |     GetPointUpdate(delta_point);
145 |     VecX gradient_pose, gradient_point;
146 |     GetJce(gradient_pose);
147 |     GetJpe(gradient_point);
148 |     VecX delta(delta_pose.size() + delta_point.size());
149 |     VecX gradient(gradient_pose.size() + gradient_point.size());
150 |     delta << delta_pose, delta_point;
151 |     gradient << gradient_pose, gradient_point;
152 |     double delta_Je = delta.dot(gradient);                  // d^T J^Te
153 |     double delta_square = delta.dot(aug_diagonal.cwiseProduct(delta));  // d^T D d
154 |     double model_change = (delta_square - delta_Je) * 0.5;
155 | 
156 |     rho_ = change / std::max(model_change, double(EPSILON));
157 | }
158 | 
159 | /*!
160 |  * @brief Stop when the magnitude of gradient (i.e. J^Te) drops below 1e-8
161 |  */
162 | bool LMBAProblem::StopCriterionGradient()
163 | {
164 |     double max_val = MaxGradient();
165 | 
166 |     if (max_val < 1e-8)
167 |     {
168 |         stream_ << "[StopCriterionGradient] Max gradient drops below 1e-8: " << max_val << "\n";
169 |         std::cout << "[StopCriterionGradient] Max gradient drops below 1e-8: " << max_val << "\n";
170 |     }
171 | 
172 |     return max_val < 1e-8;
173 | }
174 | 
175 | /*!
176 |  * @brief Stop when the relative change of parameters (i.e. delta x / x) drops below 1e-8
177 |  */
178 | bool LMBAProblem::StopCriterionUpdate()
179 | {
180 |     double max_val = Step();
181 | 
182 |     if (max_val < 1e-8)
183 |     {
184 |         stream_ << "[StopCriterionUpdate] Relative change of parameters drops below 1e-8: " << max_val << "\n";
185 |         std::cout << "[StopCriterionUpdate] Relative change of parameters drops below 1e-8: " << max_val << "\n";
186 |     }
187 | 
188 |     return max_val < 1e-8;
189 | }
190 | 
191 | bool LMBAProblem::StopCriterionRadius()
192 | {
193 |     if (mu_ < 1e-32)
194 |     {
195 |         stream_ << "[StopCriterionRadius] Trust region radius drops below 1e-32: " << mu_ << "\n";
196 |         std::cout << "[StopCriterionRadius] Trust region radius drops below 1e-32: " << mu_ << "\n";
197 |     }
198 | 
199 |     return mu_ < 1e-32;
200 | }
201 | 
202 | bool LMBAProblem::StopCriterionRelativeCostChange()
203 | {
204 |     double delta_cost = std::abs(last_square_error_ - square_error_);
205 |     double relative_cost_change = delta_cost / last_square_error_;
206 |     if (relative_cost_change < 1e-6)
207 |     {
208 |         stream_ << "[StopCriterionRelativeCostChange] Relative cost change drops below 1e-6: " << relative_cost_change << "\n";
209 |         std::cout << "[StopCriterionRelativeCostChange] Relative cost change drops below 1e-6: " << relative_cost_change << "\n";
210 |     }
211 | 
212 |     return relative_cost_change < 1e-6;
213 | }
214 | 
215 | void LMBAProblem::Print()
216 | {
217 |     double delta_loss = last_square_error_ - square_error_;
218 |     double max_gradient = MaxGradient();
219 |     double step = Step();
220 |     double mean_error, median_error, max_error;
221 |     ReprojectionError(mean_error, median_error, max_error, true);
222 |     std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
223 |     std::chrono::duration<double> elapse = now - time_;
224 |     double duration = elapse.count();
225 | 
226 |     size_t width = 9;
227 |     std::string status = step_accept_ ? std::string("[Update] ") : std::string("[Reject] ");
228 |     std::stringstream local_stream;
229 |     local_stream << std::setprecision(3) << std::scientific
230 |                  << status << std::left << std::setw(3) << iter_ << ", "
231 |                  << "d: " << std::setw(width+1) << delta_loss << ", "
232 |                  << "F0: " << std::setw(width) << last_square_error_ << ", "
233 |                  << "F1: " << std::setw(width) << square_error_ << ", "
234 |                  << "g: " << std::setw(width) << max_gradient << ", "
235 |                  << "mu: " << std::setw(width) << mu_ << ", "
236 |                  << "h: " << std::setw(width) << step << ", "
237 |                  << std::setprecision(3) << std::fixed
238 |                  << "me: " << std::setw(6) << median_error << ", "
239 |                  << "ae: " << std::setw(6) << mean_error << ", "
240 |                  << "rho: " << std::setw(5) << rho_ << ", "
241 |                  << std::setprecision(1) << std::fixed
242 |                  << "t: " << std::setw(5) << duration << "\n";
243 |     std::cout << local_stream.str();
244 |     stream_ << local_stream.str();
245 | }
246 | 
247 | double LMBAProblem::Step() const
248 | {
249 |     VecX poses, points, delta_pose, delta_point;
250 |     GetPose(poses);
251 |     GetPoint(points);
252 |     GetPoseUpdate(delta_pose);
253 |     GetPointUpdate(delta_point);
254 |     double relative_step = std::sqrt( (delta_pose.squaredNorm() + delta_point.squaredNorm()) / (poses.squaredNorm() + points.squaredNorm()) );
255 |     return relative_step;
256 | }
257 | 
258 | double LMBAProblem::MaxGradient() const
259 | {
260 |     VecX Jce, Jpe;
261 |     GetJce(Jce);
262 |     GetJpe(Jpe);
263 |     DT max_val = 0.0;
264 |     for (size_t i = 0; i < Jce.size(); i++)
265 |         max_val = std::max(max_val, std::abs(Jce(i)));
266 |     for (size_t i = 0; i < Jpe.size(); i++)
267 |         max_val = std::max(max_val, std::abs(Jpe(i)));
268 |     return max_val;
269 | }
270 | 
271 | bool LMBAProblem::StepAccept() const
272 | {
273 |     return square_error_ < last_square_error_;
274 | }
275 | 
276 | void LMBAProblem::AugmentPoseDiagonal()
277 | {
278 |     GetPoseDiagonal(pose_diagonal_);
279 |     VecX aug_pose_diagonal = pose_diagonal_ / mu_;
280 |     SetPoseDiagonal(pose_diagonal_ + aug_pose_diagonal);
281 | }
282 | 
283 | void LMBAProblem::ResetPoseDiagonal()
284 | {
285 |      SetPoseDiagonal(pose_diagonal_);
286 | }
287 | 
288 | void LMBAProblem::AugmentPointDiagonal()
289 | {
290 |     GetPointDiagonal(point_diagonal_);
291 |     VecX aug_point_diagonal = point_diagonal_ / mu_;
292 |     SetPointDiagonal(point_diagonal_ + aug_point_diagonal);
293 | }
294 | 
295 | void LMBAProblem::ResetPointDiagonal()
296 | {
297 |     SetPointDiagonal(point_diagonal_);
298 | }
299 | 
300 | 
301 | 


--------------------------------------------------------------------------------
/include/STBA/baproblem.h:
--------------------------------------------------------------------------------
  1 | #ifndef BAPROBLEM_H
  2 | #define BAPROBLEM_H
  3 | 
  4 | #include "utility.h"
  5 | #include "datablock.h"
  6 | #include "lossfunction.h"
  7 | 
  8 | #include <iostream>
  9 | #include <iomanip>
 10 | #include <chrono>
 11 | #include <ctime>
 12 | #include <Eigen/SparseCholesky>
 13 | 
 14 | enum LinearSolverType
 15 | {
 16 |     SPARSE = 0,
 17 |     DENSE = 1,
 18 |     ITERATIVE=2,
 19 |     ADAPTIVE=3
 20 | };
 21 | 
 22 | class BAProblem
 23 | {
 24 | public:
 25 |     BAProblem();
 26 |     BAProblem(LossType loss_type);
 27 |     BAProblem(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num);
 28 |     virtual ~BAProblem();
 29 | 
 30 | public:
 31 |     bool Create(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num);
 32 |     virtual bool Initialize(BundleBlock const & bundle_block);
 33 | 
 34 |     inline size_t PoseNum() const { return pose_block_.PoseNum(); }
 35 |     inline void GetPose(size_t idx, Vec3 & angle_axis, Vec3 & translation) const
 36 |     { pose_block_.GetPose(idx, angle_axis, translation);}
 37 |     inline void SetPose(size_t idx, Vec3 const & angle_axis, Vec3 const & translation)
 38 |     { pose_block_.SetPose(idx, angle_axis, translation); }
 39 | 
 40 |     inline size_t PointNum() const { return point_block_.PointNum(); }
 41 |     inline void GetPoint(size_t idx, Vec3 & point) const
 42 |     { point_block_.GetPoint(idx, point); }
 43 |     inline void SetPoint(size_t idx, Vec3 const & point)
 44 |     { point_block_.SetPoint(idx, point); }
 45 |     inline void GetColor(size_t idx, Vec3 & color) const
 46 |     { point_block_.GetColor(idx, color); }
 47 |     inline void SetColor(size_t idx, Vec3 const & color)
 48 |     { point_block_.SetColor(idx, color); }
 49 |     void SetCommonPoints(size_t, size_t, std::vector<size_t> const &);
 50 | 
 51 |     inline size_t GroupNum() const { return intrinsic_block_.GroupNum(); }
 52 |     inline void GetIntrinsic(size_t idx, Vec6 & intrinsic) const
 53 |     { intrinsic_block_.GetIntrinsic(idx, intrinsic); }
 54 |     void GetPoseIntrinsic(size_t pose_index, Vec6 & intrinsic) const
 55 |     {
 56 |         size_t group_index = GetPoseGroup(pose_index);
 57 |         GetIntrinsic(group_index, intrinsic);
 58 |     }
 59 |     inline void SetIntrinsic(size_t idx, Vec6 const & intrinsic)
 60 |     { intrinsic_block_.SetIntrinsic(idx, intrinsic); }
 61 |     void SetIntrinsic(size_t idx, size_t camera_index, Vec6 const & intrinsic);
 62 | 
 63 |     inline size_t ProjectionNum() const { return projection_block_.ProjectionNum(); }
 64 |     void SetProjection(size_t idx, size_t camera_index, size_t point_index, Vec2 const & proj);
 65 | 
 66 |     inline void SetDebugFolder(std::string const & val) { debug_folder_ = val; }
 67 |     std::string GetDebugFolder() const { return debug_folder_; }
 68 | 
 69 |     void ReprojectionError(double & mean, double & median, double & max, bool const update) const;
 70 | 
 71 |     inline void SetThreadNum(size_t val)
 72 |     {
 73 |         thread_num_ = val;
 74 | #ifdef _OPENMP
 75 |         omp_set_dynamic(0);     // Explicitly disable dynamic teams
 76 |         omp_set_num_threads(thread_num_);
 77 | #endif
 78 |         Eigen::initParallel();
 79 |         std::cout << "[SetThreadNum] Eigen thread number: " << Eigen::nbThreads() << "\n";
 80 |     }
 81 | 
 82 |     virtual void Solve() = 0;
 83 | 
 84 |     void Update(BundleBlock & bundle_block) const;
 85 | 
 86 |     void SaveReport(std::string const & report_path) const;
 87 | 
 88 | protected:
 89 |     size_t GetProjectionIndex(size_t pose_index, size_t point_index) const;
 90 |     void GetCommonPoints(size_t, size_t, std::vector<size_t> &) const;  // TODO
 91 |     void GetResidual(size_t, Vec2 &) const;
 92 |     void SetResidual(size_t, Vec2 const &);
 93 |     void GetPoseJacobian(size_t, Mat26 &) const;
 94 |     void SetPoseJacobian(size_t, Mat23 const &, Mat23 const &);
 95 |     void GetIntrinsicJacobian(size_t, Mat26 &) const;
 96 |     void SetIntrinsicJacobian(size_t, Mat26 const &);
 97 |     void GetPointJacobian(size_t, Mat23 &) const;
 98 |     void SetPointJacobian(size_t, Mat23 const &);
 99 |     void GetJcJc(size_t, Mat6 &) const;
100 |     void GetJcJc(std::vector<size_t> const &, MatX &) const;
101 |     void GetJcJc(std::vector<size_t> const & pose_indexes, SMat & JcJc) const;
102 |     void GetJcJc(MatX & JcJc) const;
103 |     void SetJcJc(size_t, Mat6 const &);
104 |     void IncreJcJc(size_t, Mat6 const &);
105 |     void GetJpJp(size_t, Mat3 &) const;
106 |     void SetJpJp(size_t, Mat3 const &);
107 |     void IncreJpJp(size_t, Mat3 const &);
108 |     void GetJcJp(size_t, Mat63 &) const;
109 |     void GetJcJp(size_t, size_t, Mat63 &) const;
110 |     void SetJcJp(size_t, Mat63 const &);
111 |     void SetJcJp(size_t, size_t, Mat63 const &);
112 |     void GetJce(size_t, Vec6 &) const;
113 |     void GetJce(std::vector<size_t> const &, VecX &) const;
114 |     void GetJce(VecX &) const;
115 |     void SetJce(size_t, Vec6 const &);
116 |     void IncreJce(size_t, Vec6 const &);
117 |     void GetJpe(size_t, Vec3 &) const;
118 |     void GetJpe(VecX &) const;
119 |     void SetJpe(size_t, Vec3 const &);
120 |     void IncreJpe(size_t, Vec3 const &);
121 |     void GetEcw(size_t, Vec6 &) const;
122 |     void GetEcw(std::vector<size_t> const &, VecX &) const;
123 |     void GetEcw(VecX & Ecw) const;
124 |     void SetECw(size_t, Vec6 const &);
125 |     void GetPose(VecX &) const;
126 |     void GetPoint(VecX &) const;
127 |     void GetPoseUpdate(VecX &) const;
128 |     void GetPointUpdate(VecX &) const;
129 |     size_t GetPoseGroup(size_t) const;
130 | 
131 | 
132 | protected:
133 |     void EvaluateResidual();
134 |     double EvaluateSquareResidual(bool const) const;
135 |     double EvaluateSquareError(bool const) const;
136 | 
137 |     void EvaluateJacobian();
138 | 
139 |     void EvaluateJcJc(size_t pose_index, Mat6 & JCJC) const;
140 |     void EvaluateJcJc();
141 |     void EvaluateJpJp(size_t point_index, Mat3 & JPJP) const;
142 |     virtual void EvaluateJpJp();
143 |     void EvaluateJcJp(size_t proj_index, Mat63 & JcJp) const;
144 |     void EvaluateJcJp(size_t pose_index, size_t point_index, Mat63 & JcJp) const;
145 |     void EvaluateJcJp();
146 | 
147 |     void EvaluateJce(size_t pose_index, Vec6 & Je) const;
148 |     void EvaluateJce(std::vector<size_t> const & pose_indexes, VecX & Je) const;
149 |     void EvaluateJce();
150 |     void EvaluateJpe(size_t point_index, Vec3 & Je) const;
151 |     void EvaluateJpe(std::vector<size_t> const & point_indexes, VecX & Je) const;
152 |     virtual void EvaluateJpe();
153 | 
154 |     void EvaluateB(MatX & B) const;
155 |     virtual bool EvaluateEcEc(size_t pose_index1, size_t pose_index2, Mat6 & EcEc) const;
156 |     virtual void EvaluateEcEc(std::vector<size_t> const & pose_indexes, MatX & EcEc) const;
157 |     void EvaluateEcEc(std::vector<size_t> const & pose_indexes, SMat & EcEc) const;
158 |     void EvaluateEcEc(MatX & EcEc) const;
159 |     void EvaluateEcEc(SMat & EcEc) const;
160 |     virtual void EvaluateEcw(size_t pose_index, Vec6 & Ecw) const;
161 |     void EvaluateEcw();
162 | 
163 |     void EvaluateSchurComplement(std::vector<size_t> const & pose_indexes, MatX & S) const;
164 |     void EvaluateSchurComplement(std::vector<size_t> const & pose_indexes, SMat & S) const;
165 |     void EvaluateSchurComplement(MatX & S) const;
166 |     void EvaluateSchurComplement(SMat & S) const;
167 |     bool EvaluateDeltaPose(std::vector<size_t> const & pose_indexes, VecX & dy) const;
168 |     bool EvaluateDeltaPose(std::vector<size_t> const & pose_indexes);
169 |     virtual bool EvaluateDeltaPose();
170 |     void EvaluateEDeltaPose(size_t point_index, Vec3 & Edy) const;
171 |     void EvaluateEDelta(size_t point_index, Vec3 & Edy) const;
172 |     virtual void EvaluateDeltaPoint(size_t point_index, Vec3 & dz);
173 |     virtual void EvaluateDeltaPoint();
174 | 
175 |     void UpdateParam();
176 | 
177 |     void ClearUpdate();
178 |     void ClearResidual();
179 |     void ClearPoseJacobian();
180 |     void ClearPointJacobian();
181 |     void ClearJcJc();
182 |     void ClearJpJp();
183 |     void ClearJcJp();
184 |     void ClearJce();
185 |     void ClearJpe();
186 |     void ClearECw();
187 |     void GetDiagonal(VecX & diagonal) const;
188 |     void SetDiagonal(VecX const & diagonal);
189 |     void GetPoseDiagonal(VecX & diagonal) const;
190 |     void SetPoseDiagonal(VecX const & diagonal);
191 |     void GetPointDiagonal(VecX & diagonal) const;
192 |     void SetPointDiagonal(VecX const & diagonal);
193 |     bool SolveLinearSystem(MatX const & A, VecX const & b, VecX & x) const;
194 |     bool SolveLinearSystem(SMat const & A, VecX const & b, VecX & x) const;
195 |     inline void SetLinearSolveType(int t) { linear_solver_type_ = static_cast<LinearSolverType>(t); }
196 | 
197 | private:
198 |     void Delete();
199 |     bool SolveLinearSystemSparse(SMat const & A, VecX const & b, VecX & x) const;
200 |     bool SolveLinearSystemDense(MatX const & A, VecX const & b, VecX & x) const;
201 |     bool SolveLinearSystemIterative(SMat const & A, VecX const & b, VecX & x) const;
202 | 
203 | protected:
204 |     PoseBlock pose_block_;
205 |     PointBlock point_block_;
206 |     IntrinsicBlock intrinsic_block_;
207 |     ProjectionBlock projection_block_;
208 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t> > pose_projection_map_;               // <pose, <point, projection> >
209 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t> > point_projection_map_;              // <point, <pose, projection> >
210 |     std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t> > > common_point_map_;    // <pose, <pose, points> >
211 |     std::unordered_map<size_t, size_t> pose_group_map_;                                                         // <pose, group>
212 |     std::unordered_map<size_t, std::vector<size_t> > group_pose_map_;                                   // <group, poses>
213 |     std::unordered_map<size_t, size_t> group_index_map_;                                                        // <local group id, origin id>
214 |     std::unordered_map<size_t, size_t> pose_index_map_;                                                          // <local pose id, origin id>
215 |     std::unordered_map<size_t, size_t> point_index_map_;                                                         // <local point id, origin id>
216 | 
217 | protected:
218 |     LossFunction * loss_function_;
219 |     DT * residual_;                             // e - reprojection error
220 |     DT * pose_jacobian_;                        // Jc, 2x6
221 |     DT * point_jacobian_;                       // Jp, 2x3
222 |     DT * pose_jacobian_square_;                 // Jc^T Jc, 6x6
223 |     DT * point_jacobian_square_;                // Jp^T Jp, 3x3
224 |     DT * pose_point_jacobian_product_;          // Jc^T Jp, 6x3
225 |     DT * pose_gradient_;                        // Jc^T e, 6x1
226 |     DT * point_gradient_;                       // Jp^T e, 3x1
227 |     DT * Ec_Cinv_w_;                            // EC^-1w, E = Jc^TJp, w = -Jp^Te (-point_gradient), 6x1
228 |     std::string debug_folder_;
229 |     size_t iter_;
230 |     bool step_accept_;
231 |     std::stringstream stream_;
232 |     std::chrono::system_clock::time_point time_;
233 |     size_t max_degree_;                         // Max degree of camera graph
234 |     size_t thread_num_;
235 |     LinearSolverType linear_solver_type_;
236 | };
237 | 
238 | #endif // BAPROBLEM_H
239 | 


--------------------------------------------------------------------------------
/src/utility.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/utility.h"
  2 | #include <chrono>
  3 | #include <vector>
  4 | #include <cassert>
  5 | #include <fstream>
  6 | #include <iostream>
  7 | #include <random>
  8 | 
  9 | DT Determinant(Mat3 const & M)
 10 | {
 11 |     double a = M(0, 0) * (M(1, 1) * M(2, 2) - M(1, 2) * M(2, 1));
 12 |     double b = M(0, 1) * (M(1, 0) * M(2, 2) - M(1, 2) * M(2, 0));
 13 |     double c = M(0, 2) * (M(1, 0) * M(2, 1) - M(1, 1) * M(2, 0));
 14 | 
 15 |     return a - b + c;
 16 | }
 17 | 
 18 | Mat3 AngleAxis2Matrix(Vec3 const & angle_axis)
 19 | {
 20 |     DT angle = std::max(angle_axis.norm(), EPSILON);
 21 |     Vec3 axis = angle_axis / angle;
 22 |     AxisAngle rotation(angle, axis);
 23 |     return rotation.toRotationMatrix();
 24 | }
 25 | 
 26 | Vec3 Quaternion2AngleAxis(Vec4 const & quaternion)
 27 | {
 28 |     DT qw = quaternion[0];
 29 |     DT qx = quaternion[1];
 30 |     DT qy = quaternion[2];
 31 |     DT qz = quaternion[3];
 32 |     DT angle = 2 * acos(qw);
 33 |     DT sq = std::max(EPSILON, 1 - qw * qw);
 34 |     DT x = qx / std::sqrt(sq);
 35 |     DT y = qy / std::sqrt(sq);
 36 |     DT z = qz / std::sqrt(sq);
 37 |     return Vec3(angle * x, angle * y, angle * z);
 38 | }
 39 | 
 40 | Vec4 AngleAxis2Quaternion(Vec3 const & angle_axis)
 41 | {
 42 |     DT angle = angle_axis.norm();
 43 |     DT sin_half_angle = std::sin(angle / 2);
 44 |     Vec3 axis = angle_axis.normalized();
 45 |     DT qw = std::cos(angle / 2);
 46 |     DT qx = axis[0] * sin_half_angle;
 47 |     DT qy = axis[1] * sin_half_angle;
 48 |     DT qz = axis[2] * sin_half_angle;
 49 |     return Vec4(qw, qx, qy, qz);
 50 | }
 51 | 
 52 | Vec3 RotatePoint(Vec3 const & angle_axis, Vec3 const & point)
 53 | {
 54 |     double angle = std::max(angle_axis.norm(), EPSILON);
 55 |     Vec3 axis = angle_axis / angle;
 56 |     double cos = std::cos(angle);
 57 |     double sin = std::sin(angle);
 58 |     Vec3 cross_prod = axis.cross(point);
 59 |     double dot_prod = axis.dot(point);
 60 |     return cos * point + sin * cross_prod + (1-cos) * dot_prod * axis;
 61 | }
 62 | 
 63 | Mat93  RotationMat2AngleAxisJacobian(Vec3 const & angle_axis)
 64 | {
 65 |     MatX jacobian_mat_angleaxis = MatX::Zero(9, 3);    // jacobian of rotation matrix w.r.t 3D angle-axis representation
 66 |     double angle = std::max(angle_axis.norm(), EPSILON);
 67 |     double x = angle_axis(0);
 68 |     double y = angle_axis(1);
 69 |     double z = angle_axis(2);
 70 |     double cos_angle = std::cos(angle);
 71 |     double sin_angle = std::sin(angle);
 72 |     double r = sin_angle / angle;
 73 |     double s = (1 - cos_angle) / (angle * angle);
 74 |     double d_r_angle = (cos_angle * angle - sin_angle) / (angle * angle);   // derivative of r w.r.t angle
 75 |     double d_s_angle = (sin_angle * angle + cos_angle * 2 - 2) / (std::pow(angle, 3));   // derivative of s w.r.t angle
 76 |     double d_angle_x = x / angle;        // derivative of angle w.r.t x
 77 |     double d_angle_y = y / angle;        // derivative of angle w.r.t y
 78 |     double d_angle_z = z / angle;        // derivative of angle w.r.t z
 79 |     double d_r_x = d_r_angle * d_angle_x;
 80 |     double d_r_y = d_r_angle * d_angle_y;
 81 |     double d_r_z = d_r_angle * d_angle_z;
 82 |     double d_s_x = d_s_angle * d_angle_x;
 83 |     double d_s_y = d_s_angle * d_angle_y;
 84 |     double d_s_z = d_s_angle * d_angle_z;
 85 |     double x2_y2 = x*x + y*y;
 86 |     double x2_z2 = x*x + z*z;
 87 |     double y2_z2 = y*y + z*z;
 88 | 
 89 |     jacobian_mat_angleaxis(0, 0) = -y2_z2 * d_s_x;
 90 |     jacobian_mat_angleaxis(0, 1) = -(2 * y * s + y2_z2 * d_s_y);
 91 |     jacobian_mat_angleaxis(0, 2) = -(2 * z * s + y2_z2 * d_s_z);
 92 |     jacobian_mat_angleaxis(1, 0) = -z * d_r_x + y * (s + x * d_s_x);
 93 |     jacobian_mat_angleaxis(1, 1) = -z * d_r_y + x * (s + y * d_s_y);
 94 |     jacobian_mat_angleaxis(1, 2) = -(r + z * d_r_z) + x * y * d_s_z;
 95 |     jacobian_mat_angleaxis(2, 0) = y * d_r_x + z * (s + x * d_s_x);
 96 |     jacobian_mat_angleaxis(2, 1) = (r + y * d_r_y) + x * z * d_s_y;
 97 |     jacobian_mat_angleaxis(2, 2) = y * d_r_z + x * (s + z * d_s_z);
 98 |     jacobian_mat_angleaxis(3, 0) = z * d_r_x + y * (s + x * d_s_x);
 99 |     jacobian_mat_angleaxis(3, 1) = z * d_r_y + x * (s + y * d_s_y);
100 |     jacobian_mat_angleaxis(3, 2) = (r + z * d_r_z) + x * y * d_s_z;
101 |     jacobian_mat_angleaxis(4, 0) = -(2 * x * s + x2_z2 * d_s_x);
102 |     jacobian_mat_angleaxis(4, 1) = -x2_z2 * d_s_y;
103 |     jacobian_mat_angleaxis(4, 2) = -(2 * z * s + x2_z2 * d_s_z);
104 |     jacobian_mat_angleaxis(5, 0) = -(r + x * d_r_x) + y * z * d_s_x;
105 |     jacobian_mat_angleaxis(5, 1) = -x * d_r_y + z * (s + y * d_s_y);
106 |     jacobian_mat_angleaxis(5, 2) = -x * d_r_z + y * (s + z * d_s_z);
107 |     jacobian_mat_angleaxis(6, 0) = -y * d_r_x + z * (s + x * d_s_x);
108 |     jacobian_mat_angleaxis(6, 1) = -(r + y * d_r_y) + x * z * d_s_y;
109 |     jacobian_mat_angleaxis(6, 2) = -y * d_r_z + x * (s + z * d_s_z);
110 |     jacobian_mat_angleaxis(7, 0) = (r + x * d_r_x) + y * z * d_s_x;
111 |     jacobian_mat_angleaxis(7, 1) = x * d_r_y + z * (s + y * d_s_y);
112 |     jacobian_mat_angleaxis(7, 2) = x * d_r_z + y * (s + z * d_s_z);
113 |     jacobian_mat_angleaxis(8, 0) = -(2 * x * s + x2_y2 * d_s_x);
114 |     jacobian_mat_angleaxis(8, 1) = -(2 * y * s + x2_y2 * d_s_y);
115 |     jacobian_mat_angleaxis(8, 2) = -x2_y2 * d_s_z;
116 | 
117 |     return jacobian_mat_angleaxis;
118 | }
119 | 
120 | /*!
121 |  * @brief Compute derivative of local point w.r.t axis-angle parameters enbodying the rotation transformation
122 |  * @brief Reference: https://math.stackexchange.com/questions/64253/jacobian-matrix-of-the-rodrigues-formula-exponential-map
123 |  * @param angle_axis: The angle-axis representation of a 3D rotation
124 |  * @param input: The input 3D vector
125 |  */
126 | Mat3 LocalPoint2RotationJacobian(Vec3 const & angle_axis, Vec3 const & input)
127 | {
128 |     MatX jacobian_out_mat = MatX::Zero(3, 9);    // jacobian of output vector w.r.t rotation matrix
129 |     //    std::cout << jacobian_out_mat << "\n";
130 |     jacobian_out_mat(0, 0) = input(0);
131 |     jacobian_out_mat(0, 1) = input(1);
132 |     jacobian_out_mat(0, 2) = input(2);
133 |     jacobian_out_mat(1, 3) = input(0);
134 |     jacobian_out_mat(1, 4) = input(1);
135 |     jacobian_out_mat(1, 5) = input(2);
136 |     jacobian_out_mat(2, 6) = input(0);
137 |     jacobian_out_mat(2, 7) = input(1);
138 |     jacobian_out_mat(2, 8) = input(2);
139 | 
140 |     Mat93 jacobian_mat_angleaxis = RotationMat2AngleAxisJacobian(angle_axis);
141 |     Mat3 jacobian = jacobian_out_mat * jacobian_mat_angleaxis;
142 |     return jacobian;
143 | }
144 | 
145 | /*!
146 |  * @brief Compute derivative of local point w.r.t translation
147 |  */
148 | Mat3 LocalPoint2TranslationJacobian()
149 | {
150 |     return Mat3::Identity();
151 | }
152 | 
153 | Mat3 LocalPoint2GlobalPointJacobian(Vec3 const & angle_axis)
154 | {
155 |     return AngleAxis2Matrix(angle_axis);
156 | }
157 | 
158 | /*!
159 |  * @brief The jacobian of projection in pixel space w.r.t distorted projetion in camera space
160 |  * u = fx' + u0, v = fy' + v0
161 |  */
162 | Mat2 Projection2DistProjectionJacobian(double const focal)
163 | {
164 |     Mat2 jacobian = Mat2::Zero();
165 |     jacobian(0, 0) = focal;
166 |     jacobian(1, 1) = focal;
167 |     return jacobian;
168 | }
169 | 
170 | /*!
171 |  * @brief The jacobian of distorted projection w.r.t the undistorted projection, both in camera space
172 |  * x' = x (1 + k1 r^2 + k2 r^4 + k3 r^6), y' = y (1 + k1 r^2 + k2 r^4 + k3 r^6), r^2 = x^2 + y^2
173 |  */
174 | Mat2 DistProjection2UndistProjectionJacobian(Vec2 const & undist_proj,
175 |                                              Vec3 const & radial_distortion)
176 | {
177 |     double r2 = undist_proj(0) * undist_proj(0) + undist_proj(1) * undist_proj(1);
178 |     double k1 = radial_distortion(0);
179 |     double k2 = radial_distortion(1);
180 |     double k3 = radial_distortion(2);
181 |     double coeff = 1 + k1 * r2 + k2 * r2 *r2 + k3 * r2 * r2 * r2;
182 |     double r2_x = 2 * undist_proj(0);
183 |     double r2_y = 2 * undist_proj(1);
184 |     Mat2 jacobian;
185 |     jacobian(0, 0) = coeff + undist_proj(0) * (k1 + 2*k2*r2 + 3*k3*r2*r2) * r2_x;
186 |     jacobian(0, 1) = undist_proj(0) * (k1 + 2*k2*r2 + 3*k3*r2*r2) * r2_y;
187 |     jacobian(1, 0) = undist_proj(1) * (k1 + 2*k2*r2 + 3*k3*r2*r2) * r2_x;
188 |     jacobian(1, 1) = coeff + undist_proj(1) * (k1 + 2*k2*r2 + 3*k3*r2*r2) * r2_y;
189 |     return jacobian;
190 | }
191 | 
192 | /*!
193 |  * @brief The jacobian of undistorted projection w.r.t local point, both in camera space
194 |  * x = X/Z, y = Y/Z
195 |  */
196 | Mat23 UndistProjection2LocalPointJacobian(Vec3 const & local_point)
197 | {
198 |     MatX jacobian = MatX::Zero(2, 3);
199 |     double x = local_point(0);
200 |     double y = local_point(1);
201 |     double z = std::max(local_point(2), EPSILON);
202 |     jacobian(0, 0) = 1 / z;
203 |     jacobian(0, 1) = 0;
204 |     jacobian(0, 2) = -x / (z * z);
205 |     jacobian(1, 0) = 0;
206 |     jacobian(1, 1) = 1 / z;
207 |     jacobian(1, 2) = -y / (z * z);
208 |     return jacobian;
209 | }
210 | 
211 | Mat26 Projection2IntrinsicJacobian(double const focal,
212 |                                    Vec3 const & radial_distortion,
213 |                                    Vec3 const & angle_axis,
214 |                                    Vec3 const & translation,
215 |                                    Vec3 const & global_point)
216 | {
217 |     Vec3 local_point = RotatePoint(angle_axis, global_point) + translation;
218 |     double depth = std::max(local_point(2), MIN_DEPTH);
219 |     double x = local_point(0) / depth;
220 |     double y = local_point(1) / depth;
221 |     double r2 = x * x + y * y;
222 |     double r4 = r2 * r2;
223 |     double r6 = r2 * r4;
224 |     double k1 = radial_distortion(0);
225 |     double k2 = radial_distortion(1);
226 |     double k3 = radial_distortion(2);
227 |     Mat26 jacobian_intrinsic = Mat26::Zero();
228 |     jacobian_intrinsic(0, 0) = x * (1 + k1 * r2 + k2 * r4 + k3 * r6);
229 |     jacobian_intrinsic(1, 0) = y * (1 + k1 * r2 + k2 * r4 + k3 * r6);
230 |     jacobian_intrinsic(0, 1) = 1;
231 |     jacobian_intrinsic(1, 2) = 1;
232 |     jacobian_intrinsic(0, 3) = x * r2 * focal;
233 |     jacobian_intrinsic(0, 4) = x * r4 * focal;
234 |     jacobian_intrinsic(0, 5) = x * r6 * focal;
235 |     jacobian_intrinsic(1, 3) = y * r2 * focal;
236 |     jacobian_intrinsic(1, 4) = y * r4 * focal;
237 |     jacobian_intrinsic(1, 5) = y * r6 * focal;
238 |     return jacobian_intrinsic;
239 | }
240 | 
241 | Mat23 Projection2RotationJacobian(double const focal,
242 |                                   Vec3 const & radial_distortion,
243 |                                   Vec3 const & angle_axis,
244 |                                   Vec3 const & translation,
245 |                                   Vec3 const & global_point)
246 | {
247 |     Vec3 local_point = RotatePoint(angle_axis, global_point) + translation;
248 |     Vec2 undist_proj(local_point(0) / std::max(local_point(2), MIN_DEPTH), local_point(1) / std::max(local_point(2), MIN_DEPTH));
249 |     Mat2 jacobian_proj_distproj = Projection2DistProjectionJacobian(focal);
250 |     Mat2 jacobian_distproj_undistproj = DistProjection2UndistProjectionJacobian(undist_proj, radial_distortion);
251 |     Mat23 jacobian_undistproj_local = UndistProjection2LocalPointJacobian(local_point);
252 |     Mat3 jacobian_local_rotation = LocalPoint2RotationJacobian(angle_axis, global_point);
253 |     Mat23 jacobian_proj_rotation = jacobian_proj_distproj * jacobian_distproj_undistproj *
254 |             jacobian_undistproj_local * jacobian_local_rotation;
255 |     return jacobian_proj_rotation;
256 | }
257 | 
258 | Mat23 Projection2TranslationJacobian(double const focal,
259 |                                      Vec3 const & radial_distortion,
260 |                                      Vec3 const & angle_axis,
261 |                                      Vec3 const & translation,
262 |                                      Vec3 const & global_point)
263 | {
264 |     Vec3 local_point = RotatePoint(angle_axis, global_point) + translation;
265 |     Vec2 undist_proj(local_point(0) / std::max(local_point(2), MIN_DEPTH), local_point(1) / std::max(local_point(2), MIN_DEPTH));
266 |     Mat2 jacobian_proj_distproj = Projection2DistProjectionJacobian(focal);
267 |     Mat2 jacobian_distproj_undistproj = DistProjection2UndistProjectionJacobian(undist_proj, radial_distortion);
268 |     Mat23 jacobian_undistproj_local = UndistProjection2LocalPointJacobian(local_point);
269 |     Mat3 jacobian_local_translation = LocalPoint2TranslationJacobian();
270 |     Mat23 jacobian_proj_translation = jacobian_proj_distproj * jacobian_distproj_undistproj *
271 |             jacobian_undistproj_local * jacobian_local_translation;
272 |     return jacobian_proj_translation;
273 | }
274 | 
275 | Mat23 Projection2GlobalPointJacobian(double const focal,
276 |                                      Vec3 const & radial_distortion,
277 |                                      Vec3 const & angle_axis,
278 |                                      Vec3 const & translation,
279 |                                      Vec3 const & global_point)
280 | {
281 |     Vec3 local_point = RotatePoint(angle_axis, global_point) + translation;
282 |     Vec2 undist_proj(local_point(0) / std::max(local_point(2), MIN_DEPTH), local_point(1) / std::max(local_point(2), MIN_DEPTH));
283 |     Mat2 jacobian_proj_distproj = Projection2DistProjectionJacobian(focal);
284 |     Mat2 jacobian_distproj_undistproj = DistProjection2UndistProjectionJacobian(undist_proj, radial_distortion);
285 |     Mat23 jacobian_undistproj_local = UndistProjection2LocalPointJacobian(local_point);
286 |     Mat3 jacobian_local_global = LocalPoint2GlobalPointJacobian(angle_axis);
287 |     Mat23 jacobian_proj_global = jacobian_proj_distproj * jacobian_distproj_undistproj *
288 |             jacobian_undistproj_local * jacobian_local_global;
289 |     return jacobian_proj_global;
290 | }
291 | 
292 | void ProjectAndGradient(Vec3 const & rotation, Vec3 const & translation, Vec3 const & point,
293 |                         double const focal, double const u, double const v,
294 |                         Vec3 const & radial_distortion, Vec2 const & projection,
295 |                         Mat23 & rotation_jacobian,
296 |                         Mat23 & translation_jacobian,
297 |                         Mat23 & point_jacobian,
298 |                         Mat26 & intrinsic_jacobian)
299 | {
300 |     rotation_jacobian = Projection2RotationJacobian(focal, radial_distortion, rotation, translation, point);
301 |     translation_jacobian = Projection2TranslationJacobian(focal, radial_distortion, rotation, translation, point);
302 |     point_jacobian = Projection2GlobalPointJacobian(focal, radial_distortion, rotation, translation, point);
303 |     intrinsic_jacobian = Projection2IntrinsicJacobian(focal, radial_distortion, rotation, translation, point);
304 | }
305 | 
306 | bool Project(double const focal, double const u, double const v,
307 |              Vec3 const & angle_axis, Vec3 const & translation,
308 |              Vec3 const & global_point,
309 |              Vec3 const & radial_distortion,
310 |              Vec2 & reprojection)
311 | {
312 |     Vec3 local_point = RotatePoint(angle_axis, global_point) + translation;
313 |     if (local_point[2] < MIN_DEPTH)
314 |     {
315 |         return false;
316 |     }
317 |     local_point[2] = std::max(local_point[2], MIN_DEPTH);
318 |     DT x = local_point[0] / local_point[2];
319 |     DT y = local_point[1] / local_point[2];
320 |     DT r2 = x * x + y * y;
321 |     DT r4 = r2 * r2;
322 |     DT r6 = r4 * r2;
323 |     DT coeff = 1 + radial_distortion(0) * r2 + radial_distortion(1) * r4 + radial_distortion(2) * r6;
324 | //    if (coeff > 1e5)
325 | //    {
326 | //        std::cout << "[Project] Too large coeff: " << coeff << "\n";
327 | //        return false;
328 | //    }
329 |     x *= coeff;
330 |     y *= coeff;
331 |     x = x * focal + u;
332 |     y = y * focal + v;
333 |     reprojection = Vec2(x, y);
334 |     return IsNumericalValid(reprojection);
335 | }
336 | 
337 | double RandomNoise(double min, double max)
338 | {
339 |     assert(min < max && "[RandomNoise] Min < Max");
340 |     double length = max - min;
341 |     return min + length * std::rand() / double(RAND_MAX);
342 | }
343 | 
344 | double GaussianNoise(double mean, double stddev)
345 | {
346 |     assert(stddev > 0 && "[GaussianNoise] stddev > 0");
347 |     unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
348 |     std::default_random_engine generator (seed);
349 |     std::normal_distribution<double> dist(mean, stddev);
350 |     return dist(generator);
351 | }
352 | 
353 | bool ReadCameraGroup(std::string const & camera_group_file,
354 |                      std::unordered_map<size_t, size_t> & camera_group_map)
355 | {
356 |     camera_group_map.clear();
357 | 
358 |     if (camera_group_file.empty())
359 |         return false;
360 | 
361 |     std::vector<std::string> lines;
362 |     if(!ReadLinesFromFile(camera_group_file, lines))
363 |         return false;
364 | 
365 |     size_t line_num = lines.size();
366 |     for(size_t i = 0; i < line_num; i++)
367 |     {
368 |         size_t camera_index, group_index;
369 |         std::stringstream camera_group_stream;
370 |         camera_group_stream << lines[i];
371 |         camera_group_stream >> camera_index;
372 |         camera_group_stream >> group_index;
373 |         camera_group_map[camera_index] = group_index;
374 |     }
375 |     return true;
376 | }
377 | 
378 | bool ReadLinesFromFile(std::string const & file_path, std::vector<std::string> & lines)
379 | {
380 |     std::ifstream stream(file_path.c_str());
381 | 
382 |     if (!stream.is_open())
383 |         return false;
384 | 
385 |     std::string line;
386 |     while (stream.good())
387 |     {
388 |         std::getline(stream, line);
389 |         if (!line.empty())
390 |             lines.push_back(line);
391 |     }
392 | 
393 |     if (!stream.eof())
394 |         return false;
395 | 
396 |     return true;
397 | }
398 | 
399 | std::string JoinPath(std::string const & folder, std::string const & file)
400 | {
401 |     std::string temp_folder = folder;
402 |     if (!folder.empty() && folder[folder.size() - 1] != '/')
403 |     {
404 |         temp_folder += "/";
405 |     }
406 |     return temp_folder + file;
407 | }
408 | 


--------------------------------------------------------------------------------
/src/dlbaproblem.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/dlbaproblem.h"
  2 | 
  3 | DLBAProblem::DLBAProblem() : min_mu_(1.0), max_mu_(1e8), mu_(1e8),
  4 |     evaluate_(true), radius_(1e4), max_iteration_(100)
  5 | {
  6 |     SetLinearSolveType(ADAPTIVE);
  7 | }
  8 | 
  9 | DLBAProblem::DLBAProblem(LossType loss_type) : BAProblem(loss_type),
 10 |     min_mu_(1.0), max_mu_(1e8), mu_(1e8), evaluate_(true), radius_(1e4), max_iteration_(100)
 11 | {
 12 |     SetLinearSolveType(ADAPTIVE);
 13 | }
 14 | 
 15 | DLBAProblem::DLBAProblem(size_t max_iter, double radius, LossType loss_type) : BAProblem(loss_type),
 16 |     min_mu_(1.0), max_mu_(1e8), mu_(1e8), evaluate_(true), radius_(radius), max_iteration_(max_iter)
 17 | {
 18 |     SetLinearSolveType(ADAPTIVE);
 19 | }
 20 | 
 21 | DLBAProblem::~DLBAProblem()
 22 | {
 23 | 
 24 | }
 25 | 
 26 | void DLBAProblem::EvaluateGradient()
 27 | {
 28 |     GetJce(pose_gradient_);
 29 |     GetJpe(point_gradient_);
 30 |     for (size_t i = 0; i < pose_gradient_.rows(); i++)
 31 |     {
 32 |         if (pose_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
 33 |             pose_gradient_(i) = 0.0;
 34 |         else
 35 |             pose_gradient_(i) = pose_gradient_(i) / pose_diagonal_sqrt_(i);
 36 |     }
 37 |     for (size_t i = 0; i < point_gradient_.rows(); i++)
 38 |     {
 39 |         if (point_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
 40 |             point_gradient_(i) = 0.0;
 41 |         else
 42 |             point_gradient_(i) = point_gradient_(i) / point_diagonal_sqrt_(i);
 43 |     }
 44 | }
 45 | 
 46 | /*!
 47 |  * @brief A cauchy step = -alpha * gradient = -ahpha * J^Te
 48 |  */
 49 | bool DLBAProblem::EvaluateCauchyStep()
 50 | {
 51 |     VecX scale_pose_gradient = pose_gradient_;
 52 |     VecX scale_point_gradient = point_gradient_;
 53 |     for (size_t i = 0; i < scale_pose_gradient.rows(); i++)
 54 |     {
 55 |         if (pose_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
 56 |             scale_pose_gradient(i) = 0.0;
 57 |         else
 58 |             scale_pose_gradient(i) = scale_pose_gradient(i) / pose_diagonal_sqrt_(i);
 59 |     }
 60 |     for (size_t i = 0; i < scale_point_gradient.rows(); i++)
 61 |     {
 62 |         if (point_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
 63 |             scale_point_gradient(i) = 0.0;
 64 |         else
 65 |             scale_point_gradient(i) = scale_point_gradient(i) / point_diagonal_sqrt_(i);
 66 |     }
 67 | 
 68 |     double jg_norm_square = 0.0;
 69 |     for (size_t i = 0; i < ProjectionNum(); i++)
 70 |     {
 71 |         Mat26 pose_jacobian;
 72 |         Mat23 point_jacobian;
 73 |         Vec6 local_pose_gradient;
 74 |         Vec3 local_point_gradient;
 75 |         Vec2 local_jacobian_gradient;
 76 |         size_t pose_index = projection_block_.PoseIndex(i);
 77 |         size_t point_index = projection_block_.PointIndex(i);
 78 |         GetPoseJacobian(i, pose_jacobian);
 79 |         GetPointJacobian(i, point_jacobian);
 80 |         local_pose_gradient = scale_pose_gradient.segment(6 * pose_index, 6);
 81 |         local_point_gradient = scale_point_gradient.segment(3 * point_index, 3);
 82 |         local_jacobian_gradient = pose_jacobian * local_pose_gradient + point_jacobian * local_point_gradient;
 83 |         jg_norm_square += local_jacobian_gradient.squaredNorm();
 84 |     }
 85 | 
 86 |     double gradient_norm_square = pose_gradient_.squaredNorm() + point_gradient_.squaredNorm();
 87 |     alpha_ = gradient_norm_square / jg_norm_square;
 88 | 
 89 |     cauchy_step_.resize(PoseNum() * 6 + PointNum() * 3);
 90 |     cauchy_step_ << (-alpha_ * pose_gradient_), (-alpha_ * point_gradient_);
 91 |     return IsNumericalValid(cauchy_step_);
 92 | }
 93 | 
 94 | bool DLBAProblem::EvaluateGaussNewtonStep()
 95 | {
 96 |     gauss_newton_step_ = VecX::Zero(PoseNum() * 6 + PointNum() * 3);
 97 | 
 98 |     bool success = false;
 99 |     while (mu_ > min_mu_)
100 |     {
101 |         AugmentPoseDiagonal();
102 |         AugmentPointDiagonal();
103 |         EvaluateEcw();
104 | 
105 |         std::vector<size_t> pose_indexes(PoseNum());
106 |         std::iota(pose_indexes.begin(), pose_indexes.end(), 0);
107 |         if (!EvaluateDeltaPose(pose_indexes))
108 |         {
109 |             std::cout << "[EvaluateGaussNewtonStep] Fail in EvaluateDeltaPose\n"
110 |                       << "Descrease mu_ from " << mu_  << " to " << 0.1 * mu_ << "\n";
111 |             mu_ *= 0.1;
112 |         }
113 |         else
114 |         {
115 |             success = true;
116 |             EvaluateDeltaPoint();
117 |             break;
118 |         }
119 |     }
120 | 
121 |     if (success)
122 |     {
123 |         for (size_t i = 0; i < PoseNum(); i++)
124 |         {
125 |             Vec6 pose_update, pose_diagonal;
126 |             pose_block_.GetDeltaPose(i, pose_update);
127 |             pose_diagonal = pose_diagonal_sqrt_.segment(6 * i, 6);
128 |             gauss_newton_step_.segment(6 * i, 6) = pose_update.cwiseProduct(pose_diagonal);
129 |         }
130 |         for (size_t i = 0; i < PointNum(); i++)
131 |         {
132 |             Vec3 point_update, point_diagonal;
133 |             point_block_.GetDeltaPoint(i, point_update);
134 |             point_diagonal = point_diagonal_sqrt_.segment(3 * i, 3);
135 |             gauss_newton_step_.segment(6 * PoseNum() + 3 * i, 3) = point_update.cwiseProduct(point_diagonal);
136 |         }
137 |     }
138 |     return success && IsNumericalValid(gauss_newton_step_);
139 | }
140 | 
141 | bool DLBAProblem::EvaluateDogLegStep()
142 | {
143 |     const double gauss_newton_norm = gauss_newton_step_.norm();
144 |     const double cauchy_norm = cauchy_step_.norm();
145 | 
146 |     if (gauss_newton_norm <= radius_)
147 |     {
148 |         // We credit he comments below to Ceres
149 |         // Case 1. The Gauss-Newton step lies inside the trust region, and
150 |         // is therefore the optimal solution to the trust-region problem.
151 |         dl_step_ = gauss_newton_step_;
152 |         dl_step_norm_ = gauss_newton_norm;
153 | //        std::cout << "Gauss-Newton step size: " << gauss_newton_norm << ", radius: " << radius_ << "\n";
154 |     }
155 |     else if (cauchy_norm >= radius_)
156 |     {
157 |         // Case 2. The Cauchy point and the Gauss-Newton steps lie outside
158 |         // the trust region. Rescale the Cauchy point to the trust region
159 |         // and return.
160 |         dl_step_ = (radius_ / cauchy_norm) * cauchy_step_;
161 |         dl_step_norm_ = radius_;
162 | //        std::cout << "Cauchy step size: " << dl_step_.norm() << ", radius: " << radius_ << "\n";
163 |     }
164 |     else
165 |     {
166 |         // Case 3. The Cauchy point is inside the trust region and the
167 |         // Gauss-Newton step is outside. Compute the line joining the two
168 |         // points and the point on it which intersects the trust region
169 |         // boundary.
170 |         // Below a means the Cauchy step, b means the Gauss-Newton step
171 |         double a_square = cauchy_step_.squaredNorm();
172 |         double c = cauchy_step_.dot(gauss_newton_step_ - cauchy_step_);
173 |         double a_minus_b_square = (gauss_newton_step_ - cauchy_step_).squaredNorm();
174 |         double d = std::sqrt(c * c -  a_minus_b_square * (a_square - radius_ * radius_));
175 |         double beta = (d - c) / a_minus_b_square;
176 | //        std::cout << "[EvaluateDogLegStep] beta = " << beta << "\n";
177 |         dl_step_ = (1.0 - beta) * cauchy_step_ + beta * gauss_newton_step_;
178 |         dl_step_norm_ = dl_step_.norm();
179 | //        std::cout << "Dogleg step size: " << dl_step_.norm() << ", cauchy step size: " << cauchy_norm
180 | //                  << ", Gauss-Newton step size: " << gauss_newton_norm << ", radius: " << radius_ << "\n";
181 |     }
182 |     for (size_t i = 0; i < 6 * PoseNum(); i++)
183 |     {
184 |         if (pose_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
185 |             dl_step_(i) = 0.0;
186 |         else
187 |             dl_step_(i) = dl_step_(i) / pose_diagonal_sqrt_(i);
188 |     }
189 |     for (size_t i = 0; i < 3 * PointNum(); i++)
190 |     {
191 |         if (point_diagonal_sqrt_(i) < std::numeric_limits<DT>::epsilon())
192 |             dl_step_(6 * PoseNum() + i) = 0.0;
193 |         else
194 |             dl_step_(6 * PoseNum() + i) =  dl_step_(6 * PoseNum() + i) / point_diagonal_sqrt_(i);
195 |     }
196 |     return IsNumericalValid(dl_step_);
197 | }
198 | 
199 | bool DLBAProblem::EvaluateStep()
200 | {
201 |     if (evaluate_)
202 |     {
203 |         GetPoseDiagonal();
204 |         GetPointDiagonal();
205 |         EvaluateGradient();
206 |         if (!EvaluateCauchyStep())
207 |         {
208 |             std::cout << "Fail in EvaluateCauchyStep.\n";
209 |             return false;
210 |         }
211 |         if (!EvaluateGaussNewtonStep())
212 |         {
213 |             std::cout << "Fail in EvaluateGaussNewtonStep.\n";
214 |             return false;
215 |         }
216 |     }
217 |     EvaluateDogLegStep();
218 | 
219 |     for (size_t i = 0; i < PoseNum(); i++)
220 |     {
221 |         Vec6 dy = dl_step_.segment(6 * i, 6);
222 |         pose_block_.SetDeltaPose(i, dy);
223 |     }
224 |     for (size_t i = 0; i < PointNum(); i++)
225 |     {
226 |         Vec3 dz = dl_step_.segment(6 * PoseNum() + 3 * i, 3);
227 |         point_block_.SetDeltaPoint(i, dz);
228 |     }
229 |     return true;
230 | }
231 | 
232 | void DLBAProblem::EvaluateRho()
233 | {
234 |     VecX diagonal = VecX::Zero(PoseNum() * 6 + PointNum() * 3);
235 |     diagonal << pose_diagonal_, point_diagonal_;
236 |     VecX aug_diagonal = diagonal / mu_;
237 |     double change = last_square_error_ - square_error_;
238 | 
239 |     VecX delta_pose, delta_point;
240 |     GetPoseUpdate(delta_pose);
241 |     GetPointUpdate(delta_point);
242 |     VecX gradient_pose, gradient_point;
243 |     GetJce(gradient_pose);
244 |     GetJpe(gradient_point);
245 |     VecX delta(delta_pose.size() + delta_point.size());
246 |     VecX gradient(gradient_pose.size() + gradient_point.size());
247 |     delta << delta_pose, delta_point;
248 |     gradient << gradient_pose, gradient_point;
249 |     double delta_Je = delta.dot(gradient);                  // d^T J^Te
250 |     double delta_square = delta.dot(aug_diagonal.cwiseProduct(delta));  // d^T D d
251 |     double model_change = (delta_square - delta_Je) * 0.5;
252 |     rho_ = change / std::max(model_change, double(EPSILON));
253 | }
254 | 
255 | bool DLBAProblem::StepAccept()
256 | {
257 |     return square_error_ < last_square_error_;
258 | }
259 | 
260 | void DLBAProblem::Solve()
261 | {
262 |     last_square_error_ = EvaluateSquareError(false);
263 |     double mean_error, median_error, max_error;
264 |     ReprojectionError(mean_error, median_error, max_error, false);
265 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
266 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
267 |     evaluate_ = true;
268 |     time_ = std::chrono::system_clock::now();
269 |     for (iter_ = 0; iter_ < max_iteration_; iter_++)
270 |     {
271 |         if (evaluate_)
272 |         {
273 |             EvaluateResidual();
274 |             EvaluateJacobian();
275 |             EvaluateJcJp();
276 |             EvaluateJcJc();
277 |             EvaluateJpJp();
278 |             EvaluateJce();
279 |             EvaluateJpe();
280 |             ClearUpdate();
281 |         }
282 | 
283 |         if (!EvaluateStep())
284 |         {
285 |             std::cout << "Fail in EvaluateStep.\n";
286 |             step_accept_ = false;
287 |         }
288 |         else
289 |         {
290 |             square_error_ = EvaluateSquareError(true);
291 |             if (StopCriterionRelativeCostChange() || StopCriterionUpdate())
292 |                 break;
293 |             step_accept_ = StepAccept();
294 |         }
295 | 
296 |         if (step_accept_)
297 |         {
298 |             EvaluateRho();
299 |             Print();
300 |             evaluate_ = true;
301 |             if (rho_ < 0.25)
302 |                 radius_ *= 0.5;
303 |             else if (rho_ > 0.75)
304 |                 radius_ = std::max(radius_, 3.0 * dl_step_norm_);
305 |             mu_ = std::min(max_mu_, 5.0 * mu_);
306 |             last_square_error_ = square_error_;
307 |             UpdateParam();
308 |         }
309 |         else
310 |         {
311 |             Print();
312 |             evaluate_ = false;
313 |             radius_ /= 3.0;
314 |         }
315 |     }
316 | 
317 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
318 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
319 |     stream_ << "[Solve] Before: mean / median / max reprojection error = "
320 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
321 |     ReprojectionError(mean_error, median_error, max_error, false);
322 |     std::cout << "[Solve] After: mean / median / max reprojection error = "
323 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
324 |     stream_ << "[Solve] After: mean / median / max reprojection error = "
325 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
326 |     stream_ << "[Setting] thread number = " << thread_num_<< "\n";
327 |     stream_ << "[Setting] Dogleg\n";
328 | }
329 | 
330 | void DLBAProblem::GetPoseDiagonal()
331 | {
332 |     size_t pose_num = PoseNum();
333 |     pose_diagonal_.resize(6 * pose_num);
334 |     pose_diagonal_sqrt_.resize(6 * pose_num);
335 |     for (size_t i = 0; i < pose_num; i++)
336 |     {
337 |         pose_diagonal_(6 * i) = pose_jacobian_square_[6 * 6 * i];
338 |         pose_diagonal_(6 * i + 1) = pose_jacobian_square_[6 * 6 * i + 7];
339 |         pose_diagonal_(6 * i + 2) = pose_jacobian_square_[6 * 6 * i + 14];
340 |         pose_diagonal_(6 * i + 3) = pose_jacobian_square_[6 * 6 * i + 21];
341 |         pose_diagonal_(6 * i + 4) = pose_jacobian_square_[6 * 6 * i + 28];
342 |         pose_diagonal_(6 * i + 5) = pose_jacobian_square_[6 * 6 * i + 35];
343 |     }
344 |     for (size_t i = 0; i < pose_diagonal_.rows(); i++)
345 |         pose_diagonal_sqrt_(i) = std::sqrt(pose_diagonal_(i));
346 | }
347 | 
348 | void DLBAProblem::GetPointDiagonal()
349 | {
350 |     size_t point_num = PointNum();
351 |     point_diagonal_.resize(3 * point_num);
352 |     point_diagonal_sqrt_.resize(3 * point_num);
353 |     for (size_t i = 0; i < point_num; i++)
354 |     {
355 |         point_diagonal_(3 * i) = point_jacobian_square_[3 * 3 * i];
356 |         point_diagonal_(3 * i + 1) = point_jacobian_square_[3 * 3 * i + 4];
357 |         point_diagonal_(3 * i + 2) = point_jacobian_square_[3 * 3 * i + 8];
358 |     }
359 |     for (size_t i = 0; i < point_diagonal_.rows(); i++)
360 |         point_diagonal_sqrt_(i) = std::sqrt(point_diagonal_(i));
361 | }
362 | 
363 | void DLBAProblem::AugmentPoseDiagonal()
364 | {
365 |     assert(pose_diagonal_.rows() == PoseNum() * 6);
366 |     VecX aug_pose_diagonal = pose_diagonal_ / mu_;
367 |     SetPoseDiagonal(pose_diagonal_ + aug_pose_diagonal);
368 | }
369 | 
370 | void DLBAProblem::ResetPoseDiagonal()
371 | {
372 |     SetPoseDiagonal(pose_diagonal_);
373 | }
374 | 
375 | void DLBAProblem::AugmentPointDiagonal()
376 | {
377 |     assert(point_diagonal_.rows() == PointNum() * 3);
378 |     VecX aug_point_diagonal = point_diagonal_ / mu_;
379 |     SetPointDiagonal(point_diagonal_ + aug_point_diagonal);
380 | }
381 | 
382 | void DLBAProblem::ResetPointDiagonal()
383 | {
384 |     SetPointDiagonal(point_diagonal_);
385 | }
386 | 
387 | double DLBAProblem::Step() const
388 | {
389 |     VecX poses, points, delta_pose, delta_point;
390 |     GetPose(poses);
391 |     GetPoint(points);
392 |     GetPoseUpdate(delta_pose);
393 |     GetPointUpdate(delta_point);
394 |     double relative_step = std::sqrt( (delta_pose.squaredNorm() + delta_point.squaredNorm()) / (poses.squaredNorm() + points.squaredNorm()) );
395 |     return relative_step;
396 | }
397 | 
398 | bool DLBAProblem::StopCriterionUpdate()
399 | {
400 |     double max_val = Step();
401 | 
402 |     if (max_val < 1e-8)
403 |     {
404 |         stream_ << "[StopCriterionUpdate] Relative change of parameters drops below 1e-8: " << max_val << "\n";
405 |         std::cout << "[StopCriterionUpdate] Relative change of parameters drops below 1e-8: " << max_val << "\n";
406 |     }
407 | 
408 |     return max_val < 1e-8;
409 | }
410 | 
411 | bool DLBAProblem::StopCriterionRelativeCostChange()
412 | {
413 |     double delta_cost = std::abs(last_square_error_ - square_error_);
414 |     double relative_cost_change = delta_cost / last_square_error_;
415 |     if (relative_cost_change < 1e-6)
416 |     {
417 |         stream_ << "[StopCriterionRelativeCostChange] Relative cost change drops below 1e-6: " << relative_cost_change << "\n";
418 |         std::cout << "[StopCriterionRelativeCostChange] Relative cost change drops below 1e-6: " << relative_cost_change << "\n";
419 |     }
420 | 
421 |     return relative_cost_change < 1e-6;
422 | }
423 | 
424 | void DLBAProblem::Print()
425 | {
426 |     double delta_loss = last_square_error_ - square_error_;
427 |     double mean_error, median_error, max_error;
428 |     ReprojectionError(mean_error, median_error, max_error, true);
429 |     std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
430 |     std::chrono::duration<double> elapse = now - time_;
431 |     double duration = elapse.count();
432 | 
433 |     size_t width = 9;
434 |     std::string status = step_accept_ ? std::string("[Update] ") : std::string("[Reject] ");
435 |     std::stringstream local_stream;
436 |     local_stream << std::setprecision(3) << std::scientific
437 |                  << status << std::left << std::setw(3) << iter_ << ", "
438 |                  << "d: " << std::setw(width+1) << delta_loss << ", "
439 |                  << "F0: " << std::setw(width) << last_square_error_ << ", "
440 |                  << "F1: " << std::setw(width) << square_error_ << ", "
441 |                  << "radius: " << std::setw(width) << radius_ << ", "
442 |                  << "mu: " << std::setw(width) << mu_ << ", "
443 |                  << "rho: " << std::setw(width) << rho_ << ", "
444 |                  << std::setprecision(3) << std::fixed
445 |                  << "me: " << std::setw(6) << median_error << ", "
446 |                  << std::setprecision(1) << std::fixed
447 |                  << "t: " << std::setw(5) << duration << "\n";
448 |     std::cout << local_stream.str();
449 |     stream_ << local_stream.str();
450 | }
451 | 


--------------------------------------------------------------------------------
/src/datablock.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/datablock.h"
  2 | 
  3 | #include <fstream>
  4 | 
  5 | std::vector<size_t> BundleBlock::GroupIndexes() const
  6 | {
  7 |     std::vector<size_t> indexes;
  8 |     indexes.reserve(groups_.size());
  9 |     std::unordered_map<size_t, DGroup>::const_iterator it = groups_.begin();
 10 |     for (; it != groups_.end(); it++)
 11 |     {
 12 |         indexes.push_back(it->first);
 13 |     }
 14 |     return indexes;
 15 | }
 16 | 
 17 | std::vector<size_t> BundleBlock::CameraIndexes() const
 18 | {
 19 |     std::vector<size_t> indexes;
 20 |     indexes.reserve(cameras_.size());
 21 |     std::unordered_map<size_t, DCamera>::const_iterator it = cameras_.begin();
 22 |     for (; it != cameras_.end(); it++)
 23 |     {
 24 |         indexes.push_back(it->first);
 25 |     }
 26 |     return indexes;
 27 | }
 28 | 
 29 | std::vector<size_t> BundleBlock::TrackIndexes() const
 30 | {
 31 |     std::vector<size_t> indexes;
 32 |     indexes.reserve(tracks_.size());
 33 |     std::unordered_map<size_t, DTrack>::const_iterator it = tracks_.begin();
 34 |     for (; it != tracks_.end(); it++)
 35 |     {
 36 |         indexes.push_back(it->first);
 37 |     }
 38 |     return indexes;
 39 | }
 40 | 
 41 | std::vector<size_t> BundleBlock::ProjectionIndexes() const
 42 | {
 43 |     std::vector<size_t> indexes;
 44 |     indexes.reserve(projections_.size());
 45 |     std::unordered_map<size_t, DProjection>::const_iterator it = projections_.begin();
 46 |     for (; it != projections_.end(); it++)
 47 |     {
 48 |         indexes.push_back(it->first);
 49 |     }
 50 |     return indexes;
 51 | }
 52 | 
 53 | BundleBlock::DGroup const & BundleBlock::GetGroup(size_t id) const
 54 | {
 55 |     std::unordered_map<size_t, DGroup>::const_iterator it = groups_.find(id);
 56 |     assert(it != groups_.end() && "[BundleBlock::GetGroup] Group id not found");
 57 |     return it->second;
 58 | }
 59 | 
 60 | BundleBlock::DGroup & BundleBlock::GetGroup(size_t id)
 61 | {
 62 |     std::unordered_map<size_t, DGroup>::iterator it = groups_.find(id);
 63 |     assert(it != groups_.end() && "[BundleBlock::GetGroup] Group id not found");
 64 |     return it->second;
 65 | }
 66 | 
 67 | BundleBlock::DCamera const & BundleBlock::GetCamera(size_t id) const
 68 | {
 69 |     std::unordered_map<size_t, DCamera>::const_iterator it = cameras_.find(id);
 70 |     assert(it != cameras_.end() && "[BundleBlock::GetCamera] Camera id not found");
 71 |     return it->second;
 72 | }
 73 | 
 74 | BundleBlock::DCamera & BundleBlock::GetCamera(size_t id)
 75 | {
 76 |     std::unordered_map<size_t, DCamera>::iterator it = cameras_.find(id);
 77 |     assert(it != cameras_.end() && "[BundleBlock::GetCamera] Camera id not found");
 78 |     return it->second;
 79 | }
 80 | 
 81 | BundleBlock::DTrack const & BundleBlock::GetTrack(size_t id) const
 82 | {
 83 |     std::unordered_map<size_t, DTrack>::const_iterator it = tracks_.find(id);
 84 |     assert(it != tracks_.end() && "[BundleBlock::GetTrack] Track id not found");
 85 |     return it->second;
 86 | }
 87 | 
 88 | BundleBlock::DTrack & BundleBlock::GetTrack(size_t id)
 89 | {
 90 |     std::unordered_map<size_t, DTrack>::iterator it = tracks_.find(id);
 91 |     assert(it != tracks_.end() && "[BundleBlock::GetTrack] Track id not found");
 92 |     return it->second;
 93 | }
 94 | 
 95 | BundleBlock::DProjection const & BundleBlock::GetProjection(size_t id) const
 96 | {
 97 |     std::unordered_map<size_t, DProjection>::const_iterator it = projections_.find(id);
 98 |     assert(it != projections_.end() && "[BundleBlock::GetProjection] Projection id not found");
 99 |     return it->second;
100 | }
101 | 
102 | BundleBlock::DProjection & BundleBlock::GetProjection(size_t id)
103 | {
104 |     std::unordered_map<size_t, DProjection>::iterator it = projections_.find(id);
105 |     assert(it != projections_.end() && "[BundleBlock::GetProjection] Projection id not found");
106 |     return it->second;
107 | }
108 | 
109 | void BundleBlock::GetCommonPoints(std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t> > > & common_point_map) const
110 | {
111 |     std::vector<size_t> camera_indexes = CameraIndexes();
112 |     for (size_t i = 0; i < camera_indexes.size(); i++)
113 |     {
114 |         size_t camera_index = camera_indexes[i];
115 |         DCamera const & camera = GetCamera(camera_index);
116 | 
117 |         std::unordered_map<size_t, std::vector<size_t> > local_map;
118 |         local_map[camera_index] = std::vector<size_t>();
119 |         std::unordered_set<size_t> const & link_camera_indexes = camera.linked_cameras;
120 |         std::unordered_set<size_t>::const_iterator it = link_camera_indexes.begin();
121 |         for (; it != link_camera_indexes.end(); it++)
122 |         {
123 |             size_t link_camera_index = *it;
124 |             local_map[link_camera_index] = std::vector<size_t>();
125 |         }
126 |         common_point_map[camera_index] = local_map;
127 |     }
128 | 
129 |     std::vector<size_t> track_indexes = TrackIndexes();
130 |     for (size_t i = 0; i < track_indexes.size(); i++)
131 |     {
132 |         size_t track_index = track_indexes[i];
133 |         DTrack const & track = GetTrack(track_index);
134 | 
135 |         std::unordered_set<size_t> const & proj_indexes = track.linked_projections;
136 |         std::vector<size_t> local_camera_indexes;
137 |         std::unordered_set<size_t>::const_iterator it = proj_indexes.begin();
138 |         for (; it != proj_indexes.end(); it++)
139 |         {
140 |             size_t proj_index = *it;
141 |             DProjection const & projection = GetProjection(proj_index);
142 |             local_camera_indexes.push_back(projection.camera_id);
143 |         }
144 |         for (size_t j = 0; j < local_camera_indexes.size(); j++)
145 |         {
146 |             size_t camera_index1 = local_camera_indexes[j];
147 |             std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t> > >::iterator it1;
148 |             it1 = common_point_map.find(camera_index1);
149 |             assert(it1 != common_point_map.end() && "[BundleBlock::GetCommonPoints] Camera index1 not found");
150 |             std::unordered_map<size_t, std::vector<size_t> > & local_map = it1->second;
151 |             for (size_t k = 0; k < local_camera_indexes.size(); k++)
152 |             {
153 |                 size_t camera_index2 = local_camera_indexes[k];
154 |                 std::unordered_map<size_t, std::vector<size_t> >::iterator it2 = local_map.find(camera_index2);
155 |                 assert(it2 != local_map.end() && "[BundleBlock::GetCommonPoints] Camera index2 not found");
156 |                 std::vector<size_t> & common_points = it2->second;
157 |                 common_points.push_back(track_index);
158 |             }
159 |         }
160 |     }
161 | }
162 | 
163 | bool BundleBlock::LoadColmapTxt(std::string const & cameras_path, std::string const & images_path, std::string const & points_path)
164 | {
165 |     // Read camera intrinsics
166 |     std::cout << "[BundleBlock::LoadColmapTxt] Load intrinsics: " << cameras_path << "\n";
167 |     {
168 |         std::ifstream cameras_file_stream(cameras_path, std::ios::in);
169 |         if (!cameras_file_stream)
170 |             return false;
171 | 
172 |         while (!cameras_file_stream.eof() && !cameras_file_stream.bad())
173 |         {
174 |             std::string line;
175 |             std::getline(cameras_file_stream, line);
176 |             if (line.size() == 0 || line[0] == '#')
177 |             {
178 |                 continue;
179 |             }
180 | 
181 |             std::istringstream line_stream(line);
182 |             size_t cam_idx;
183 |             std::string model_name;
184 |             int width, height;
185 |             double fx, fy, u, v;
186 |             double k1 = 0, k2 = 0, p1 = 0, p2 = 0, k3 = 0, k4 = 0, sx1 = 0, sy1 = 0;
187 | 
188 |             line_stream >> cam_idx >> model_name >> width >> height;
189 |             if (model_name != "PINHOLE"
190 |                     && model_name != "THIN_PRISM_FISHEYE"
191 |                     && model_name != "SIMPLE_RADIAL"
192 |                     && model_name != "OPENCV")
193 |             {
194 |                 std::cout << "Cannot deal with camera models other than PINHOLE and THIN_PRISM_FISHEYE."
195 |                           << std::endl;
196 |                 return false;
197 |             }
198 |             if (model_name == "PINHOLE")
199 |             {
200 |                 line_stream >> fx >> fy >> u >> v;
201 |             }
202 |             else if (model_name == "THIN_PRISM_FISHEYE")
203 |             {
204 |                 line_stream >> fx >> fy >> u >> v;
205 |                 line_stream >> k1 >> k2 >> p1 >> p2 >> k3 >> k4 >> sx1 >> sy1;
206 |             }
207 |             else if (model_name == "SIMPLE_RADIAL")
208 |             {
209 |                 line_stream >> fx >> u >> v >> k1;
210 |                 fy = fx;
211 |             }
212 |             else if (model_name == "OPENCV")
213 |             {
214 |                 line_stream >>  fx >> fy >> u >> v >> k1 >> k2 >> p1 >> p2;
215 |             }
216 | 
217 |             Vec6 intrinsic;
218 |             intrinsic << fx, u, v, k1, k2, k3;
219 |             groups_[cam_idx] = DGroup(cam_idx, intrinsic, width, height);
220 |         }
221 |         cameras_file_stream.close();
222 |     }
223 | 
224 |     // Read camera extrinsics
225 |     std::cout << "[BundleBlock::LoadColmapTxt] Load extrinsics: " << images_path << "\n";
226 |     {
227 |         std::ifstream images_file_stream(images_path, std::ios::in);
228 |         if (!images_file_stream)
229 |         {
230 |             return false;
231 |         }
232 | 
233 |         size_t projection_id = 0;
234 |         while (!images_file_stream.eof() && !images_file_stream.bad())
235 |         {
236 |             std::string line;
237 |             std::getline(images_file_stream, line);
238 |             if (line.size() == 0 || line[0] == '#')
239 |             {
240 |                 continue;
241 |             }
242 | 
243 |             // Read image info line.
244 |             size_t image_idx, camera_idx;
245 |             double qw, qx, qy, qz, tx, ty, tz;
246 |             std::string image_path;
247 | 
248 |             std::istringstream image_stream(line);
249 |             image_stream >> image_idx >> qw >> qx >> qy >> qz
250 |                     >> tx >> ty >> tz >> camera_idx >> image_path;
251 | 
252 |             Vec3 angle_axis = Quaternion2AngleAxis(Vec4(qw, qx, qy, qz));
253 | 
254 |             DCamera camera(image_idx, camera_idx, angle_axis, Vec3(tx, ty, tz), image_path);
255 | 
256 |             // read projections
257 |             std::unordered_set<size_t> track_ids;
258 |             std::getline(images_file_stream, line);
259 |             std::istringstream observations_stream(line);
260 |             while (!observations_stream.eof() && !observations_stream.bad())
261 |             {
262 |                 double px, py;
263 |                 int track_idx;
264 |                 observations_stream >> px >> py >> track_idx;
265 | 
266 |                 if (track_idx == -1 || track_ids.find(track_idx) != track_ids.end()) continue;
267 | 
268 |                 // init track
269 |                 if (tracks_.find(track_idx) == tracks_.end())
270 |                 {
271 |                     tracks_[track_idx] = DTrack(track_idx);
272 |                 }
273 | 
274 |                 // add projection
275 |                 DProjection projection(projection_id, image_idx, track_idx, Vec2(px, py));
276 |                 projections_[projection_id] = projection;
277 |                 camera.linked_projections.insert(projection_id);
278 |                 DTrack & track = tracks_[track_idx];
279 |                 track.linked_projections.insert(projection_id);
280 |                 projection_id++;
281 |             }
282 |             cameras_[image_idx] = camera;
283 |         }
284 |     }
285 | 
286 |     // Read points
287 |     std::cout << "[BundleBlock::LoadColmapTxt] Load points: " << points_path << "\n";
288 |     {
289 |         std::ifstream point_file_stream(points_path, std::ios::in);
290 |         if (!point_file_stream)
291 |         {
292 |             return false;
293 |         }
294 |         while (!point_file_stream.eof() && !point_file_stream.bad())
295 |         {
296 |             std::string line;
297 |             std::getline(point_file_stream, line);
298 |             if (line.size() == 0 || line[0] == '#')
299 |             {
300 |                 continue;
301 |             }
302 | 
303 |             // Read image info line.
304 |             size_t track_idx;
305 |             double px, py, pz, cx, cy, cz;
306 | 
307 |             // set intrisic from intrisic_xms
308 |             std::istringstream track_stream(line);
309 |             track_stream >> track_idx >> px >> py >> pz >> cx >> cy >> cz;
310 |             if (tracks_.find(track_idx) == tracks_.end())
311 |             {
312 |                 std::cout << "cannot find the track " << track_idx << std::endl;
313 |                 continue;
314 |             }
315 |             DTrack & track = tracks_[track_idx];
316 |             track.position = Vec3(px, py, pz);
317 |             track.color = Vec3(cx, cy, cz);
318 |         }
319 |     }
320 | 
321 |     // Build linked cameras
322 |     {
323 |         std::vector<size_t> track_indexes = TrackIndexes();
324 |         for (size_t i = 0; i < track_indexes.size(); i++)
325 |         {
326 |             size_t track_index = track_indexes[i];
327 |             DTrack const & track = GetTrack(track_index);
328 |             std::unordered_set<size_t> const & linked_projections = track.linked_projections;
329 |             std::unordered_set<size_t>::const_iterator it = linked_projections.begin();
330 |             std::unordered_set<size_t> linked_cameras;
331 |             for (; it != linked_projections.end(); it++)
332 |             {
333 |                 size_t proj_index = *it;
334 |                 DProjection const & projection = GetProjection(proj_index);
335 |                 linked_cameras.insert(projection.camera_id);
336 |             }
337 |             std::unordered_set<size_t>::const_iterator it1 = linked_cameras.begin();
338 |             for (; it1 != linked_cameras.end(); it1++)
339 |             {
340 |                 size_t camera_index1 = *it1;
341 |                 DCamera & camera1 = GetCamera(camera_index1);
342 |                 std::unordered_set<size_t>::const_iterator it2 = linked_cameras.begin();
343 |                 for (; it2 != linked_cameras.end(); it2++)
344 |                 {
345 |                     size_t camera_index2 = *it2;
346 |                     if (camera_index1 != camera_index2)
347 |                         camera1.linked_cameras.insert(camera_index2);
348 |                 }
349 |             }
350 |         }
351 |     }
352 | 
353 |     Print();
354 | 
355 |     return true;
356 | }
357 | 
358 | void BundleBlock::SaveColmapTxt(std::string const & cameras_path, std::string const & images_path, std::string const & points_path) const
359 | {
360 |     // save camera intrinsics
361 |     {
362 |         std::ofstream stream(cameras_path);
363 |         std::unordered_map<size_t, DGroup>::const_iterator it = groups_.begin();
364 |         for (; it != groups_.end(); it++)
365 |         {
366 |             DGroup const & group = it->second;
367 |             /* some mis-alignment here.
368 |              * Our intrinsic is parameterized by <focal u v k1 k2 k3>, while the OPENCV model is <fx fy u v k1 k2 p1 p2>
369 |              */
370 |             stream << group.id << " OPENCV " << group.width << " " << group.height << " " << group.intrinsic[0] << " " << group.intrinsic[0] << " "
371 |                    << group.intrinsic[1] << " " << group.intrinsic[2] << " " << group.intrinsic[3] << " " << group.intrinsic[4] << " 0 0\n";
372 |         }
373 |         stream.close();
374 |     }
375 | 
376 |     // save camera extrinsics
377 |     {
378 |         std::ofstream stream(images_path);
379 |         std::unordered_map<size_t, DCamera>::const_iterator it = cameras_.begin();
380 |         for (; it != cameras_.end(); it++)
381 |         {
382 |             DCamera const & camera = it->second;
383 |             Vec4 quaternion = AngleAxis2Quaternion(camera.axis_angle);
384 |             stream << camera.id << " ";
385 |             for (size_t i = 0; i < 4; i++)  stream << quaternion[i] << " ";
386 |             for (size_t i = 0; i < 3; i++) stream << camera.translation[i] << " ";
387 |             stream << camera.group_id << " " << camera.image_path << "\n";
388 |             std::unordered_set<size_t> const & linked_projections = camera.linked_projections;
389 |             std::unordered_set<size_t>::const_iterator it = linked_projections.begin();
390 |             for (; it != linked_projections.end(); it++)
391 |             {
392 |                 size_t proj_index = *it;
393 |                 DProjection const & projection = GetProjection(proj_index);
394 |                 stream << projection.projection[0] << " " << projection.projection[1] << " " << projection.track_id << " ";
395 |             }
396 |             stream << "\n";
397 |         }
398 |         stream.close();
399 |     }
400 | 
401 |     // save points
402 |     {
403 |         std::ofstream stream(points_path);
404 |         std::unordered_map<size_t, DTrack>::const_iterator it = tracks_.begin();
405 |         for (; it != tracks_.end(); it++)
406 |         {
407 |             DTrack const & track = it->second;
408 |             /* some mis-alignment here.
409 |              * The error and the track info are not saved here, since they are a bit redundant.
410 |              */
411 |             stream << track.id << " ";
412 |             for (size_t i = 0; i < 3; i++)  stream << track.position[i] << " ";
413 |             for (size_t i = 0; i < 3; i++)  stream << track.color[i] << " ";
414 |             stream << "\n";
415 |         }
416 |         stream.close();
417 |     }
418 | }
419 | 
420 | void BundleBlock::Print() const
421 | {
422 |     std::cout << "[BundleBlock::Print] # groups = " << groups_.size() << "\n"
423 |               << "# cameras = " << cameras_.size() << "\n"
424 |               << "# tracks = " << tracks_.size() << "\n"
425 |               << "# projections = " << projections_.size() << "\n";
426 | }
427 | 
428 | void BundleBlock::AddGaussianNoiseToTrack(DT mean, DT sigma)
429 | {
430 |     std::cout << "[BundleBlock::AddGaussianNoiseToTrack] mean = " << mean << ", sigma = " << sigma << "\n";
431 |     std::unordered_map<size_t, DTrack>::iterator it = tracks_.begin();
432 |     for (; it != tracks_.end(); it++)
433 |     {
434 |         DTrack & track = it->second;
435 |         for (size_t i = 0; i < 3; i++)
436 |             track.position[i] += GaussianNoise(mean, sigma);
437 |     }
438 | }
439 | 
440 | void BundleBlock::AddGaussianNoiseToCameraTranslation(DT mean, DT sigma)
441 | {
442 |     std::cout << "[BundleBlock::AddGaussianNoiseToCameraTranslation] mean = " << mean << ", sigma = " << sigma << "\n";
443 |     std::unordered_map<size_t, DCamera>::iterator it = cameras_.begin();
444 |     for (; it != cameras_.end(); it++)
445 |     {
446 |         DCamera & camera = it->second;
447 |         for (size_t i = 0; i < 3; i++)
448 |             camera.translation[i] += GaussianNoise(mean, sigma);
449 |     }
450 | }
451 | 


--------------------------------------------------------------------------------
/include/STBA/datablock.h:
--------------------------------------------------------------------------------
  1 | #ifndef DATABLOCK_H
  2 | #define DATABLOCK_H
  3 | 
  4 | #include "utility.h"
  5 | #include <unordered_set>
  6 | #include <unordered_map>
  7 | #include <vector>
  8 | #include <cassert>
  9 | #include <cstdlib>
 10 | #include <iostream>
 11 | 
 12 | class PoseBlock
 13 | {
 14 | public:
 15 |     PoseBlock() : poses_(NULL), delta_poses_(NULL), temp_delta_poses_(NULL), pose_num_(0) {}
 16 |     PoseBlock(size_t pose_num) : pose_num_(pose_num)
 17 |     {
 18 |         poses_ = new DT[pose_num * 6];
 19 |         delta_poses_ = new DT[pose_num * 6];
 20 |         temp_delta_poses_ = new DT[pose_num * 6];
 21 |         std::fill(delta_poses_, delta_poses_ + pose_num * 6, 0.0);
 22 |     }
 23 |     ~PoseBlock()
 24 |     {
 25 |         if (poses_ != NULL)             delete [] poses_;
 26 |         if (delta_poses_ != NULL)       delete [] delta_poses_;
 27 |         if (temp_delta_poses_ != NULL)  delete [] temp_delta_poses_;
 28 |     }
 29 | 
 30 |     bool Create(size_t pose_num)
 31 |     {
 32 |         if (poses_ != NULL)         delete [] poses_;
 33 |         if (delta_poses_ != NULL)   delete [] delta_poses_;
 34 |         if (temp_delta_poses_ != NULL)  delete [] temp_delta_poses_;
 35 |         try
 36 |         {
 37 |             poses_ = new DT[pose_num * 6];
 38 |             delta_poses_ = new DT[pose_num * 6];
 39 |             temp_delta_poses_ = new DT[pose_num * 6];
 40 |         }
 41 |         catch (std::bad_alloc & e)
 42 |         {
 43 |             std::cout << "[PoseBlock::Create] Catching bad_alloc: " << e.what() << std::endl;
 44 |             return false;
 45 |         }
 46 |         std::fill(delta_poses_, delta_poses_ + pose_num * 6, 0.0);
 47 |         pose_num_ = pose_num;
 48 |         return true;
 49 |     }
 50 | 
 51 |     inline size_t PoseNum() const { return pose_num_; }
 52 |     inline void SetPose(size_t idx, Vec3 const & angle_axis, Vec3 const & translation)
 53 |     {
 54 |         assert(idx < pose_num_ && "Pose index out of range");
 55 |         DT* ptr = poses_ + idx * 6;
 56 |         Eigen::Map<Vec6> pose(ptr);
 57 |         pose.head<3>() = angle_axis;
 58 |         pose.tail<3>() = translation;
 59 |     }
 60 |     inline void GetPose(size_t idx, Vec3 & angle_axis, Vec3 & translation) const
 61 |     {
 62 |         assert(idx < pose_num_ && "Pose index out of range");
 63 |         DT* ptr = poses_ + idx * 6;
 64 |         angle_axis = Vec3(ptr);        // first rotation, second translation
 65 |         translation = Vec3(ptr + 3);
 66 |     }
 67 |     inline void GetPose(size_t idx, Vec6 & pose) const
 68 |     {
 69 |         assert(idx < pose_num_ && "Pose index out of range");
 70 |         DT* ptr = poses_ + idx * 6;
 71 |         pose = Vec6(ptr);
 72 |     }
 73 |     inline void SetDeltaPose(size_t idx, Vec6 const & dy)
 74 |     {
 75 |         assert(idx < pose_num_ && "Pose index out of range");
 76 |         DT* ptr = delta_poses_ + idx * 6;
 77 |         Eigen::Map<Vec6> pose(ptr);
 78 |         pose = dy;
 79 |     }
 80 |     inline void SetDeltaPose(size_t idx, Vec3 const & angle_axis, Vec3 const & translation)
 81 |     {
 82 |         assert(idx < pose_num_ && "Pose index out of range");
 83 |         DT* ptr = delta_poses_ + idx * 6;
 84 |         Eigen::Map<Vec6> pose(ptr);
 85 |         pose.head<3>() = angle_axis;
 86 |         pose.tail<3>() = translation;
 87 |     }
 88 |     inline void IncreDeltaPose(size_t idx, Vec3 const & angle_axis, Vec3 const & translation)
 89 |     {
 90 |         assert(idx < pose_num_ && "Pose index out of range");
 91 |         DT* ptr = delta_poses_ + idx * 6;
 92 |         Eigen::Map<Vec6> pose(ptr);
 93 |         pose.head<3>() += angle_axis;
 94 |         pose.tail<3>() += translation;
 95 |     }
 96 |     inline void GetDeltaPose(size_t idx, Vec3 & angle_axis, Vec3 & translation) const
 97 |     {
 98 |         assert(idx < pose_num_ && "Pose index out of range");
 99 |         DT* ptr = delta_poses_ + idx * 6;
100 |         angle_axis = Vec3(ptr);        // first rotation, second translation
101 |         translation = Vec3(ptr + 3);
102 |     }
103 |     inline void GetDeltaPose(size_t idx, Vec6 & dy) const
104 |     {
105 |         assert(idx < pose_num_ && "Pose index out of range");
106 |         DT* ptr = delta_poses_ + idx * 6;
107 |         dy = Vec6(ptr);
108 |     }
109 |     inline void SetTempDeltaPose(size_t idx, Vec3 const & angle_axis, Vec3 const & translation)
110 |     {
111 |         assert(idx < pose_num_ && "Pose index out of range");
112 |         DT* ptr = temp_delta_poses_ + idx * 6;
113 |         Eigen::Map<Vec6> pose(ptr);
114 |         pose.head<3>() = angle_axis;
115 |         pose.tail<3>() = translation;
116 |     }
117 |     inline void GetTempDeltaPose(size_t idx, Vec3 & angle_axis, Vec3 & translation) const
118 |     {
119 |         assert(idx < pose_num_ && "Pose index out of range");
120 |         DT* ptr = temp_delta_poses_ + idx * 6;
121 |         angle_axis = Vec3(ptr);        // first rotation, second translation
122 |         translation = Vec3(ptr + 3);
123 |     }
124 |     inline void GetTempDeltaPose(size_t idx, Vec6 & dy) const
125 |     {
126 |         assert(idx < pose_num_ && "Pose index out of range");
127 |         DT* ptr = temp_delta_poses_ + idx * 6;
128 |         dy = Vec6(ptr);
129 |     }
130 |     inline void AverageDeltaPose(size_t const batch_size)
131 |     {
132 |         for (size_t i = 0; i < pose_num_ * 6; i++)
133 |         {
134 |             delta_poses_[i] /= DT(batch_size);
135 |             temp_delta_poses_[i] = delta_poses_[i];
136 |         }
137 |     }
138 | 
139 |     void UpdatePose()
140 |     {
141 |         for (size_t i = 0; i < pose_num_ * 6; i++)
142 |             poses_[i] += delta_poses_[i];
143 |     }
144 |     void ClearUpdate()
145 |     {
146 |         std::fill(delta_poses_, delta_poses_ + pose_num_ * 6, 0.0);
147 |     }
148 | 
149 | private:
150 |     DT* poses_;
151 |     DT* delta_poses_;
152 |     DT* temp_delta_poses_;
153 |     size_t pose_num_;
154 | };
155 | 
156 | class PointBlock
157 | {
158 | public:
159 |     PointBlock() : points_(NULL), delta_points_(NULL), point_num_(0) {}
160 |     PointBlock(size_t point_num) : point_num_(point_num)
161 |     {
162 |         points_ = new DT[point_num * 3];
163 |         delta_points_ = new DT[point_num * 3];
164 |         colors_ = new DT[point_num * 3];
165 |         std::fill(delta_points_, delta_points_ + point_num * 3, 0.0);
166 |     }
167 |     ~PointBlock()
168 |     {
169 |         if (points_ != NULL) delete [] points_;
170 |         if (delta_points_ != NULL) delete [] delta_points_;
171 |         if (colors_ != NULL) delete [] colors_;
172 |     }
173 | 
174 |     bool Create(size_t point_num)
175 |     {
176 |         if (points_ != NULL) delete [] points_;
177 |         if (delta_points_ != NULL) delete [] delta_points_;
178 |         try
179 |         {
180 |             points_ = new DT[point_num * 3];
181 |             delta_points_ = new DT[point_num * 3];
182 |         }
183 |         catch (std::bad_alloc & e)
184 |         {
185 |             std::cout << "[PointBlock::Create] Catching bad_alloc: " << e.what() << std::endl;
186 |             return false;
187 |         }
188 |         std::fill(delta_points_, delta_points_ + point_num * 3, 0.0);
189 |         colors_ = new DT[point_num * 3];
190 |         point_num_ = point_num;
191 |         return true;
192 |     }
193 |     inline size_t PointNum() const { return point_num_; }
194 |     inline void SetPoint(size_t idx, Vec3 const & pt)
195 |     {
196 |         assert(idx < point_num_ && "Point index out of range");
197 |         DT* ptr = points_ + idx * 3;
198 |         Eigen::Map<Vec3> point(ptr);
199 |         point = pt;
200 |     }
201 |     inline void GetPoint(size_t idx, Vec3 & pt) const
202 |     {
203 |         DT* ptr = points_ + idx * 3;
204 |         pt = Vec3(ptr);
205 |     }
206 |     inline void SetDeltaPoint(size_t idx, Vec3 const & pt)
207 |     {
208 |         assert(idx < point_num_ && "Point index out of range");
209 |         DT* ptr = delta_points_ + idx * 3;
210 |         Eigen::Map<Vec3> point(ptr);
211 |         point = pt;
212 |     }
213 |     inline void IncreDeltaPoint(size_t idx, Vec3 const & pt)
214 |     {
215 |         assert(idx < point_num_ && "Point index out of range");
216 |         DT* ptr = delta_points_ + idx * 3;
217 |         Eigen::Map<Vec3> point(ptr);
218 |         point += pt;
219 |     }
220 |     inline void GetDeltaPoint(size_t idx, Vec3 & pt) const
221 |     {
222 |         DT* ptr = delta_points_ + idx * 3;
223 |         pt = Vec3(ptr);
224 |     }
225 |     inline void AverageDeltaPoint(size_t const batch_size)
226 |     {
227 |         for (size_t i = 0; i < point_num_ * 3; i++)
228 |             delta_points_[i] /= DT(batch_size);
229 |     }
230 |     void UpdatePoint()
231 |     {
232 |         for (size_t i = 0; i < point_num_ * 3; i++)
233 |             points_[i] += delta_points_[i];
234 |     }
235 |     void ClearUpdate()
236 |     {
237 |         std::fill(delta_points_, delta_points_ + point_num_ * 3, 0.0);
238 |     }
239 |     inline void SetColor(size_t idx, Vec3 const & color)
240 |     {
241 |         assert(idx < point_num_ && "Point index out of range");
242 |         DT* ptr = colors_ + idx * 3;
243 |         Eigen::Map<Vec3> clr(ptr);
244 |         clr = color;
245 |     }
246 |     inline void GetColor(size_t idx, Vec3 & color) const
247 |     {
248 |         DT* ptr = colors_ + idx * 3;
249 |         color = Vec3(ptr);
250 |     }
251 | 
252 | private:
253 |     DT* points_;
254 |     DT* delta_points_;
255 |     DT* colors_;
256 |     size_t point_num_;
257 | };
258 | 
259 | class IntrinsicBlock
260 | {
261 | public:
262 |     IntrinsicBlock() : intrinsics_(NULL), delta_intrinsics_(NULL), group_num_(0) {}
263 |     IntrinsicBlock(size_t group_num) : group_num_(group_num)
264 |     {
265 |         intrinsics_ = new DT[group_num * 6];    // focal, u, v, radial_distortion
266 |         delta_intrinsics_ = new DT[group_num * 6];
267 |         std::fill(delta_intrinsics_, delta_intrinsics_ + group_num * 6, 0.0);
268 |     }
269 |     ~IntrinsicBlock()
270 |     {
271 |         if (intrinsics_ != NULL)             delete intrinsics_;
272 |         if (delta_intrinsics_ != NULL)       delete delta_intrinsics_;
273 |     }
274 |     bool Create(size_t group_num)
275 |     {
276 |         if (intrinsics_ != NULL)             delete intrinsics_;
277 |         if (delta_intrinsics_ != NULL)       delete delta_intrinsics_;
278 |         try
279 |         {
280 |             intrinsics_ = new DT[group_num * 6];
281 |             delta_intrinsics_ = new DT[group_num * 6];
282 |         }
283 |         catch (std::bad_alloc & e)
284 |         {
285 |             std::cout << "[IntrinsicBlock::Create] Catching bad_alloc: " << e.what() << std::endl;
286 |             return false;
287 |         }
288 |         std::fill(delta_intrinsics_, delta_intrinsics_ + group_num * 6, 0.0);
289 |         group_num_ = group_num;
290 |         return true;
291 |     }
292 |     inline size_t GroupNum() const { return group_num_; }
293 |     inline void SetIntrinsic(size_t idx, Vec6 const & intr)
294 |     {
295 |         assert(idx < group_num_ && "Group index out of range");
296 |         DT* ptr = intrinsics_ + idx * 6;
297 |         Eigen::Map<Vec6> intrinsic(ptr);
298 |         intrinsic = intr;
299 |     }
300 |     inline void GetIntrinsic(size_t idx, Vec6 & intr) const
301 |     {
302 |         assert(idx < group_num_ && "Group index out of range");
303 |         DT* ptr = intrinsics_ + idx * 6;
304 |         intr = Vec6(ptr);
305 |     }
306 |     inline void SetDeltaIntrinsic(size_t idx, Vec6 const & intr)
307 |     {
308 |         assert(idx < group_num_ && "Group index out of range");
309 |         DT* ptr = delta_intrinsics_ + idx * 6;
310 |         Eigen::Map<Vec6> intrinsic(ptr);
311 |         intrinsic = intr;
312 |     }
313 |     inline void GetDeltaIntrinsic(size_t idx, Vec6 & intr) const
314 |     {
315 |         assert(idx < group_num_ && "Group index out of range");
316 |         DT* ptr = delta_intrinsics_ + idx * 6;
317 |         intr = Vec6(ptr);
318 |     }
319 |     void UpdateIntrinsics()
320 |     {
321 |         for (size_t i = 0; i < group_num_ * 6; i++)
322 |             intrinsics_[i] += delta_intrinsics_[i];
323 |     }
324 |     void ClearUpdate()
325 |     {
326 |         std::fill(delta_intrinsics_, delta_intrinsics_ + group_num_ * 6, 0.0);
327 |     }
328 | 
329 | private:
330 |     DT * intrinsics_;
331 |     DT * delta_intrinsics_;
332 |     size_t group_num_;
333 | };
334 | 
335 | class ProjectionBlock
336 | {
337 | public:
338 |     ProjectionBlock() : projections_(NULL), indexes_(NULL), projection_num_(0) {}
339 |     ProjectionBlock(size_t proj_num) : projection_num_(proj_num)
340 |     {
341 |         projections_ = new DT[proj_num * 2];
342 |         indexes_ = new size_t[proj_num * 2];
343 |     }
344 |     ~ProjectionBlock()
345 |     {
346 |         if (projections_ != NULL) delete [] projections_;
347 |         if (indexes_ != NULL) delete [] indexes_;
348 |     }
349 | 
350 |     bool Create(size_t proj_num)
351 |     {
352 |         if (projections_ != NULL) delete [] projections_;
353 |         if (indexes_ != NULL) delete [] indexes_;
354 |         try
355 |         {
356 |             projections_ = new DT[proj_num * 2];
357 |             indexes_ = new size_t[proj_num * 2];
358 |         }
359 |         catch (std::bad_alloc & e)
360 |         {
361 |             std::cout << "[ProjectionBlock::Create] Catching bad_alloc: " << e.what() << std::endl;
362 |             return false;
363 |         }
364 |         projection_num_ = proj_num;
365 |         return true;
366 |     }
367 |     inline size_t ProjectionNum() const { return projection_num_; }
368 |     inline void SetProjection(size_t idx, size_t camera_index, size_t point_index, Vec2 const & proj)
369 |     {
370 |         assert(idx < projection_num_ && "Projection index out of range");
371 |         indexes_[2 * idx] = camera_index;
372 |         indexes_[2 * idx + 1] = point_index;
373 |         projections_[2 * idx] = proj(0);
374 |         projections_[2 * idx + 1] = proj(1);
375 |     }
376 |     inline void GetProjection(size_t idx, Vec2 & proj) const
377 |     {
378 |         DT* ptr = projections_ + idx * 2;
379 |         proj = Vec2(ptr);
380 |     }
381 |     inline size_t PoseIndex(size_t idx) const
382 |     {
383 |         assert(idx < projection_num_ && "Projection index out of range");
384 |         return indexes_[2 * idx];
385 |     }
386 |     inline size_t PointIndex(size_t idx) const
387 |     {
388 |         assert(idx < projection_num_ && "Projection index out of range");
389 |         return indexes_[2 * idx + 1];
390 |     }
391 | 
392 | private:
393 |     DT * projections_;
394 |     size_t * indexes_;
395 |     size_t projection_num_;
396 | };
397 | 
398 | class PointMeta
399 | {
400 | public:
401 |     PointMeta() : cluster_num_(0), data_(NULL) {}
402 |     PointMeta(size_t cluster_num) : cluster_num_(cluster_num), data_(NULL)
403 |     {
404 |         data_ = new DT[cluster_num * 15];
405 |         std::fill(data_, data_ + cluster_num * 15, 0.0);
406 |     }
407 |     ~PointMeta()
408 |     {
409 |         if (data_ != NULL)  delete data_;
410 |     }
411 |     void GetJpJp(size_t cluster_index, Mat3 & JpJp) const
412 |     {
413 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetJpJp] Cluster index out of range");
414 |         DT * ptr = data_ + cluster_index * 15;
415 |         JpJp = Mat3(ptr);
416 |     }
417 |     void SetJpJp(size_t cluster_index, Mat3 const & JpJp)
418 |     {
419 |         assert(cluster_index < cluster_num_ && "[MPointMeta::SetJpJp] Cluster index out of range");
420 |         for (size_t i = 0; i < 3; i++)
421 |             for (size_t j = 0; j < 3; j++)
422 |                 data_[cluster_index * 15 + i * 3 + j] = JpJp(i, j);
423 |     }
424 |     void GetDiagonal(size_t cluster_index, Vec3 & diagonal) const
425 |     {
426 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetDiagonal] Cluster index out of range");
427 |         for (size_t i = 0; i < 3; i++)
428 |             diagonal(i) = data_[cluster_index * 15 + i * 4];
429 |     }
430 |     void SetDiagonal(size_t cluster_index, Vec3 const & diagonal)
431 |     {
432 |         assert(cluster_index < cluster_num_ && "[MPointMeta::SetJpJp] Cluster index out of range");
433 |         for (size_t i = 0; i < 3; i++)
434 |             data_[cluster_index * 15 + i * 4] = diagonal(i);
435 |     }
436 |     void AddDiagonal(size_t cluster_index, Vec3 const & diagonal)
437 |     {
438 |         assert(cluster_index < cluster_num_ && "[MPointMeta::SetJpJp] Cluster index out of range");
439 |         for (size_t i = 0; i < 3; i++)
440 |             data_[cluster_index * 15 + i * 4] += diagonal(i);
441 |     }
442 |     void GetJpe(size_t cluster_index, Vec3 & Jpe) const
443 |     {
444 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetJpe] Cluster index out of range");
445 |         DT * ptr = data_ + cluster_index * 15 + 9;
446 |         Jpe = Vec3(ptr);
447 |     }
448 |     void SetJpe(size_t cluster_index, Vec3 const & Jpe)
449 |     {
450 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetJpe] Cluster index out of range");
451 |         for (size_t i = 0; i < 3; i++)
452 |             data_[cluster_index * 15 + 9 + i] = Jpe(i);
453 |     }
454 |     void GetDeltaPoint(size_t cluster_index, Vec3 & dz) const
455 |     {
456 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetDeltaPoint] Cluster index out of range");
457 |         DT * ptr = data_ + cluster_index * 15 + 12;
458 |         dz = Vec3(ptr);
459 |     }
460 |     void SetDeltaPoint(size_t cluster_index, Vec3 const & dz)
461 |     {
462 |         assert(cluster_index < cluster_num_ && "[MPointMeta::GetDeltaPoint] Cluster index out of range");
463 |         for (size_t i = 0; i < 3; i++)
464 |             data_[cluster_index * 15 + 12 + i] = dz(i);
465 |     }
466 | 
467 | private:
468 |     size_t cluster_num_;
469 |     DT * data_; // JpJp, Jpe, dz
470 | };
471 | 
472 | class BundleBlock
473 | {
474 | public:
475 |     struct DGroup
476 |     {
477 |         DGroup() {}
478 |         DGroup(size_t i, Vec6 const & intri) : id (i), intrinsic(intri) {}
479 |         DGroup(size_t i, Vec6 const & intri, int w, int h) : id (i), intrinsic(intri), width(w), height(h) {}
480 | 
481 |         void Print() const
482 |         {
483 |             std::cout << "[Group] " << id << "\n" << "intrinsic: " << intrinsic << "\n";
484 |         }
485 | 
486 |         size_t id;
487 |         Vec6 intrinsic; // focal, u0, v0, radial_distortion[3]
488 |         int width;
489 |         int height;
490 |     };
491 | 
492 |     struct DCamera
493 |     {
494 |         DCamera() {}
495 |         DCamera(size_t i, size_t g_id, Vec3 const & aa, Vec3 const & trans) :
496 |             id(i), group_id(g_id), axis_angle(aa), translation(trans) {}
497 |         DCamera(size_t i, size_t g_id, Vec3 const & aa, Vec3 const & trans, std::string const & path) :
498 |             id(i), group_id(g_id), axis_angle(aa), translation(trans), image_path(path) {}
499 | 
500 | 
501 |         void Print() const
502 |         {
503 |             std::cout << "[Camera] " << id << "\n"
504 |                       << "axis angle: " << axis_angle << "\n" << "translation: " << translation << "\n";
505 |         }
506 | 
507 |         size_t id;
508 |         size_t group_id;
509 |         std::unordered_set<size_t> linked_projections;
510 |         std::unordered_set<size_t> linked_cameras;
511 |         Vec3 axis_angle;
512 |         Vec3 translation;
513 | 
514 |         std::string image_path;
515 |     };
516 |     struct DTrack
517 |     {
518 |         DTrack() {}
519 |         DTrack(size_t i) : id(i) {}
520 |         DTrack(size_t i, Vec3 const & pos) :
521 |             id(i), position(pos) {}
522 |         DTrack(size_t i, Vec3 const & pos, Vec3 const & c) :
523 |             id(i), position(pos), color(c) {}
524 | 
525 |         void Print() const
526 |         {
527 |             std::cout << "[Track] " << id << "\n" << "position: " << position << "\n";
528 |         }
529 | 
530 |         size_t id;
531 |         std::unordered_set<size_t> linked_projections;
532 |         Vec3 position;
533 |         Vec3 color;
534 |     };
535 |     struct DProjection
536 |     {
537 |         DProjection() {}
538 |         DProjection(size_t i, size_t ci, size_t ti, Vec2 const & proj) :
539 |             id(i), camera_id(ci), track_id(ti), projection(proj) {}
540 | 
541 |         void Print() const
542 |         {
543 |             std::cout << "[Projection] " << id << "\n" << "camera id: " << camera_id << "\n"
544 |                       << "track id:" << track_id << "\n" << "projection: " << projection << "\n";
545 |         }
546 | 
547 |         size_t id;
548 |         size_t camera_id;
549 |         size_t track_id;
550 |         Vec2 projection;
551 |     };
552 | 
553 | public:
554 |     std::vector<size_t> GroupIndexes() const;
555 |     std::vector<size_t> CameraIndexes() const;
556 |     std::vector<size_t> TrackIndexes() const;
557 |     std::vector<size_t> ProjectionIndexes() const;
558 | 
559 |     DGroup const & GetGroup(size_t id) const;
560 |     DGroup & GetGroup(size_t id);
561 |     DCamera const & GetCamera(size_t id) const;
562 |     DCamera & GetCamera(size_t id);
563 |     DTrack const & GetTrack(size_t id) const;
564 |     DTrack & GetTrack(size_t id);
565 |     DProjection const & GetProjection(size_t id) const;
566 |     DProjection & GetProjection(size_t id);
567 | 
568 |     inline void InsertGroup(DGroup const & group)               { groups_[group.id] = group; }
569 |     inline void InsertCamera(DCamera const & camera)        { cameras_[camera.id] = camera; }
570 |     inline void InsertTrack(DTrack const & track)                   { tracks_[track.id] = track; }
571 |     inline void InsertProjection(DProjection const & projection)
572 |     {
573 |         projections_[projection.id] = projection;
574 |         if (cameras_.find(projection.camera_id) != cameras_.end())
575 |         {
576 |             DCamera & camera = cameras_[projection.camera_id];
577 |             camera.linked_projections.insert(projection.id);
578 |         }
579 |         if (tracks_.find(projection.track_id) != tracks_.end())
580 |         {
581 |             DTrack & track = tracks_[projection.track_id];
582 |             track.linked_projections.insert(projection.id);
583 |         }
584 |     }
585 | 
586 |     void GetCommonPoints(std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t> > > & common_point_map) const;
587 | 
588 |     bool LoadColmapTxt(std::string const & cameras_path, std::string const & images_path, std::string const & points_path);
589 | 
590 |     void SaveColmapTxt(std::string const & cameras_path, std::string const & images_path, std::string const & points_path) const;
591 | 
592 |     void Print() const;
593 | 
594 |     void AddGaussianNoiseToTrack(DT mean, DT sigma);
595 | 
596 |     void AddGaussianNoiseToCameraTranslation(DT mean, DT sigma);
597 | 
598 | private:
599 |     std::unordered_map<size_t, DGroup> groups_;
600 |     std::unordered_map<size_t, DCamera> cameras_;
601 |     std::unordered_map<size_t, DTrack> tracks_;
602 |     std::unordered_map<size_t, DProjection> projections_;
603 | };
604 | 
605 | #endif // DATABLOCK_H
606 | 
607 | 


--------------------------------------------------------------------------------
/src/clustering/louvain.cpp:
--------------------------------------------------------------------------------
  1 | #include "STBA/clustering/louvain.h"
  2 | #include "STBA/utility.h"
  3 | 
  4 | #include <queue>
  5 | #include <chrono>
  6 | #include <unordered_map>
  7 | #include <iostream>
  8 | 
  9 | size_t RandomPick(std::unordered_map<size_t, double> const & prob_map)
 10 | {
 11 |     assert(!prob_map.empty());
 12 |     double sum = 0.0;
 13 |     std::unordered_map<size_t, double>::const_iterator it = prob_map.begin();
 14 |     for (; it != prob_map.end(); it++)
 15 |     {
 16 |         sum += it->second;
 17 |     }
 18 | 
 19 |     double r = ((double) rand() / (RAND_MAX)) * sum;
 20 |     double accu_sum = 0.0;
 21 |     it = prob_map.begin();
 22 |     size_t index = it->first;
 23 |     for (; it != prob_map.end(); it++)
 24 |     {
 25 |         accu_sum += it->second;
 26 |         if (accu_sum >= r)
 27 |         {
 28 |             index = it->first;
 29 |             break;
 30 |         }
 31 |     }
 32 |     return index;
 33 | }
 34 | 
 35 | Louvain::Louvain() : max_community_(std::numeric_limits<size_t>::max()), temperature_(10.0)
 36 | {
 37 | 
 38 | }
 39 | 
 40 | Louvain::Louvain(std::vector<size_t> const & nodes,
 41 |                  std::unordered_map<size_t, std::unordered_map<size_t, double> > const & edges)
 42 |     : max_community_(std::numeric_limits<size_t>::max()), temperature_(10.0)
 43 | {
 44 |     Initialize(nodes, edges);
 45 | }
 46 | 
 47 | Louvain::Louvain(std::vector<size_t> const & nodes,
 48 |                  std::vector<std::pair<size_t, size_t> > const & edges)
 49 |     : max_community_(std::numeric_limits<size_t>::max()), temperature_(10.0)
 50 | {
 51 |     Initialize(nodes, edges);
 52 | }
 53 | 
 54 | Louvain::~Louvain()
 55 | {
 56 | 
 57 | }
 58 | 
 59 | void Louvain::Initialize(std::vector<size_t> const & nodes,
 60 |                          std::vector<std::pair<size_t, size_t> > const & edges)
 61 | {
 62 |     size_t node_num = nodes.size();
 63 |     graph_.Resize(node_num);
 64 |     node_graph_.Resize(node_num);
 65 |     node_map_.resize(node_num);
 66 |     community_map_.resize(node_num);
 67 |     community_size_.resize(node_num);
 68 |     community_in_weight_.resize(node_num);
 69 |     community_total_weight_.resize(node_num);
 70 | 
 71 |     std::unordered_map<size_t, size_t> node_index_map;
 72 |     for (size_t i = 0; i < node_num; i++)
 73 |     {
 74 |         node_index_map[nodes[i]] = i;
 75 |         double node_weight = 0.0;
 76 |         graph_.AddSelfEdge(node_weight);
 77 |         node_graph_.AddSelfEdge(node_weight);
 78 |         node_map_[i] = i;
 79 |         community_map_[i] = i;
 80 |         community_size_[i] = 1;
 81 |         community_in_weight_[i] = node_weight * 2;
 82 |         community_total_weight_[i] = node_weight * 2;
 83 |     }
 84 | 
 85 |     sum_edge_weight_ = 0.0;
 86 |     for (size_t i = 0; i < edges.size(); i++)
 87 |     {
 88 |         std::pair<size_t, size_t> const & edge = edges[i];
 89 |         size_t index1 = edge.first;
 90 |         size_t index2 = edge.second;
 91 |         assert(node_index_map.find(index1) != node_index_map.end());
 92 |         assert(node_index_map.find(index2) != node_index_map.end());
 93 |         size_t node_index1 = node_index_map[index1];
 94 |         size_t node_index2 = node_index_map[index2];
 95 |         if (node_index1 > node_index2)  continue;
 96 |         double weight = 1.0;
 97 |         graph_.AddUndirectedEdge(node_index1, node_index2, weight);
 98 |         node_graph_.AddUndirectedEdge(node_index1, node_index2, weight);
 99 | 
100 |         sum_edge_weight_ += 2 * weight;
101 |         community_total_weight_[node_index1] += weight;
102 |         community_total_weight_[node_index2] += weight;
103 |     }
104 | 
105 |     double avg_weight = sum_edge_weight_ / edges.size();
106 |     for (size_t i = 0; i < node_num; i++)
107 |     {
108 |         node_graph_.AddSelfEdge(i, avg_weight);
109 |         graph_.AddSelfEdge(i, avg_weight);
110 |     }
111 | 
112 |     std::cout << "------------------ Build graph --------------------\n"
113 |               << "# nodes: " << graph_.NodeNum() << ", # edges: " << edges.size() << "\n"
114 |               << "---------------------------------------------------\n";
115 | }
116 | 
117 | void Louvain::Initialize(std::vector<size_t> const & nodes,
118 |                          std::unordered_map<size_t, std::unordered_map<size_t, double> > const & edges)
119 | {
120 |     size_t node_num = nodes.size();
121 |     graph_.Resize(node_num);
122 |     node_graph_.Resize(node_num);
123 |     node_map_.resize(node_num);
124 |     community_map_.resize(node_num);
125 |     community_size_.resize(node_num);
126 |     community_in_weight_.resize(node_num);
127 |     community_total_weight_.resize(node_num);
128 | 
129 |     std::unordered_map<size_t, size_t> node_index_map;
130 |     for (size_t i = 0; i < node_num; i++)
131 |     {
132 |         node_index_map[nodes[i]] = i;
133 |         double node_weight = 0.0;
134 |         graph_.AddSelfEdge(node_weight);
135 |         node_graph_.AddSelfEdge(node_weight);
136 |         node_map_[i] = i;
137 |         community_map_[i] = i;
138 |         community_size_[i] = 1;
139 |         community_in_weight_[i] = node_weight * 2;
140 |         community_total_weight_[i] = node_weight * 2;
141 |     }
142 | 
143 |     sum_edge_weight_ = 0.0;
144 |     size_t edge_num = 0;
145 |     std::unordered_map<size_t, std::unordered_map<size_t, double> >::const_iterator it1 = edges.begin();
146 |     for (; it1 != edges.end(); it1++)
147 |     {
148 |         size_t index1 = it1->first;
149 |         assert(node_index_map.find(index1) != node_index_map.end());
150 |         size_t node_index1 = node_index_map[index1];
151 |         std::unordered_map<size_t, double> const & submap = it1->second;
152 |         std::unordered_map<size_t, double>::const_iterator it2 = submap.begin();
153 |         for (; it2 != submap.end(); it2++)
154 |         {
155 |             size_t index2 = it2->first;
156 |             if (index1 == index2)   continue;
157 |             edge_num++;
158 |            assert(node_index_map.find(index2) != node_index_map.end());
159 |            size_t node_index2 = node_index_map[index2];
160 |            if (node_index1 > node_index2)   continue;
161 |             double weight = it2->second;
162 | 
163 |             graph_.AddUndirectedEdge(node_index1, node_index2, weight);
164 |             node_graph_.AddUndirectedEdge(node_index1, node_index2, weight);
165 | 
166 |             sum_edge_weight_ += 2 * weight;
167 |             community_total_weight_[node_index1] += weight;
168 |             community_total_weight_[node_index2] += weight;
169 |         }
170 |     }
171 | 
172 |     double avg_weight = sum_edge_weight_ / edge_num;
173 |     for (size_t i = 0; i < node_num; i++)
174 |     {
175 |         node_graph_.AddSelfEdge(i, avg_weight);
176 |         graph_.AddSelfEdge(i, avg_weight);
177 |     }
178 | 
179 |     std::cout << "------------------ Build graph --------------------\n"
180 |               << "# nodes: " << nodes.size() << ", # edges: " << edge_num << "\n"
181 |               << "---------------------------------------------------\n";
182 | }
183 | 
184 | void Louvain::Reinitialize()
185 | {
186 |     graph_ = node_graph_;
187 | 
188 |     size_t node_num = node_graph_.NodeNum();
189 |     node_map_.resize(node_num);
190 |     community_map_.resize(node_num);
191 |     community_size_.resize(node_num);
192 |     community_in_weight_.resize(node_num);
193 |     community_total_weight_.resize(node_num);
194 |     for (size_t i = 0; i < node_num; i++)
195 |     {
196 |         double node_weight = 0.0;
197 |         node_map_[i] = i;
198 |         community_map_[i] = i;
199 |         community_size_[i] = 1;
200 |         community_in_weight_[i] = node_weight * 2;
201 |         community_total_weight_[i] = node_weight * 2;
202 |     }
203 | }
204 | 
205 | double Louvain::Modularity(size_t const community) const
206 | {
207 |     assert(community < community_in_weight_.size());
208 |     double in_weight = community_in_weight_[community];
209 |     double total_weight = community_total_weight_[community];
210 |     double modularity = in_weight / sum_edge_weight_ - std::pow(total_weight / sum_edge_weight_, 2);
211 |     return modularity;
212 | }
213 | 
214 | double Louvain::Modularity() const
215 | {
216 |     double modularity = 0.0;
217 |     size_t num_communities = community_in_weight_.size();
218 |     for (size_t i = 0; i < num_communities; i++)
219 |     {
220 |         double in_weight = community_in_weight_[i];
221 |         double total_weight = community_total_weight_[i];
222 |         modularity += in_weight / sum_edge_weight_ - std::pow(total_weight / sum_edge_weight_, 2);
223 |     }
224 |     return modularity;
225 | }
226 | 
227 | double Louvain::EdgeWeight(size_t const index1, size_t const index2) const
228 | {
229 |     return node_graph_.EdgeWeight(index1, index2);
230 | }
231 | 
232 | void Louvain::GetClusters(std::vector<std::vector<size_t> > & clusters) const
233 | {
234 |     clusters.clear();
235 |     size_t num_communities = community_in_weight_.size();
236 |     size_t num_nodes = node_map_.size();
237 |     clusters.resize(num_communities, std::vector<size_t>());
238 |     for (size_t i = 0; i < num_nodes; i++)
239 |     {
240 |         size_t community_index = node_map_[i];
241 |         assert(community_index < num_communities && "[GetClusters] Community index out of range");
242 |         clusters[community_index].push_back(i);
243 |     }
244 | }
245 | 
246 | void Louvain::GetEdgesAcrossClusters(std::vector<std::pair<size_t, size_t> > & pairs) const
247 | {
248 |     pairs.clear();
249 | 
250 |     size_t node_num = node_graph_.NodeNum();
251 |     for (size_t i = 0; i < node_num; i++)
252 |     {
253 |         size_t node_index1 = i;
254 |         size_t community_index1 = node_map_[node_index1];
255 |         std::vector<EdgeData> const & edges = node_graph_.GetIncidentEdges(i);
256 |         for (size_t j = 0; j < edges.size(); j++)
257 |         {
258 |             EdgeData const & edge = edges[j];
259 |             size_t node_index2 = edge.node;
260 |             size_t community_index2 = node_map_[node_index2];
261 |             if (node_index1 > node_index2)
262 |                 continue;
263 |             if (community_index1 != community_index2)
264 |             {
265 |                 pairs.push_back(std::make_pair(node_index1, node_index2));
266 |             }
267 |         }
268 |     }
269 | }
270 | 
271 | void Louvain::Print()
272 | {
273 |     std::cout << "------------------------ Print ------------------------\n";
274 |     std::cout << "# communities = " << community_in_weight_.size() << ", modularity = " << Modularity() << "\n";
275 | 
276 |     std::vector<std::vector<size_t> > clusters;
277 |     GetClusters(clusters);
278 |     for (size_t i = 0; i < clusters.size(); i++)
279 |     {
280 |         std::vector<size_t> const & nodes = clusters[i];
281 |         std::cout << "Community " << i << " of size " << nodes.size() << ": ";
282 |         for (size_t j = 0; j < nodes.size(); j++)
283 |         {
284 |             size_t node_index = nodes[j];
285 |              std::cout << node_index << " ";
286 |         }
287 |         std::cout << "\n";
288 |     }
289 |     std::cout << "-------------------------------------------------------\n";
290 | }
291 | 
292 | void Louvain::Cluster()
293 | {
294 |     size_t pass = 0;
295 | 
296 |     while(Merge())
297 |     {
298 |         Rebuild();
299 |         pass++;
300 |     }
301 | }
302 | 
303 | /*!
304 |  * @param initial_pairs - The end points of initial pairs must be in the same cluster.
305 |  */
306 | void Louvain::Cluster(std::vector<std::pair<size_t, size_t> > const & initial_pairs)
307 | {
308 |     size_t pass = 0;
309 | 
310 |     Merge(initial_pairs);
311 |     Rebuild();
312 | 
313 |     while(Merge())
314 |     {
315 |         Rebuild();
316 |         pass++;
317 |     }
318 | }
319 | 
320 | /*!
321 |  * @brief StochasticCluster introduces stochasticity to clustering by merging clusters with some probability
322 |  * rather than greedily like louvain's algorithm.
323 |  */
324 | void Louvain::StochasticCluster()
325 | {
326 |     size_t pass = 0;
327 | 
328 |     std::srand(unsigned(std::time(0)));
329 |     while (StochasticMerge())
330 |     {
331 |         Rebuild();
332 |         pass++;
333 |     }
334 | }
335 | 
336 | void Louvain::StochasticCluster(std::vector<std::pair<size_t, size_t> > const & initial_pairs)
337 | {
338 |     size_t pass = 0;
339 | 
340 |     Merge(initial_pairs);
341 |     Rebuild();
342 | 
343 |     std::srand(unsigned(std::time(0)));
344 |     while (StochasticMerge())
345 |     {
346 |         Rebuild();
347 |         pass++;
348 |     }
349 | }
350 | 
351 | bool Louvain::Merge()
352 | {
353 |     bool improved = false;
354 |     std::queue<size_t> queue;
355 | 
356 |     size_t node_num = graph_.NodeNum();
357 |     std::unordered_map<size_t, bool> visited;
358 |     for (size_t i = 0; i < node_num; i++)
359 |     {
360 |         size_t community_index = i;
361 |         if (community_size_[community_index] < max_community_)
362 |         {
363 |             queue.push(community_index);
364 |             visited[community_index] = false;
365 |         }
366 |         else
367 |         {
368 |             visited[community_index] = true;
369 |         }
370 |     }
371 | 
372 |      std::vector<size_t> prev_community_size = community_size_;
373 | 
374 |     size_t loop_count = 0;
375 |     while(!queue.empty())
376 |     {
377 |         size_t node_index = queue.front();
378 |         queue.pop();
379 |         visited[node_index] = true;
380 | 
381 |         double self_weight = graph_.GetSelfWeight(node_index);
382 |         double total_weight = self_weight;
383 | 
384 |         std::vector<EdgeData> const & edges = graph_.GetIncidentEdges(node_index);
385 |         std::unordered_map<size_t, double> neighb_weights;
386 |         for (size_t i = 0; i < edges.size(); i++)
387 |         {
388 |             EdgeData const & edge = edges[i];
389 |             size_t neighb_index = edge.node;
390 |             size_t neighb_community_index = community_map_[neighb_index];
391 |             neighb_weights[neighb_community_index] += edge.weight;
392 |             total_weight += edge.weight;
393 |         }
394 | 
395 |         size_t prev_community = community_map_[node_index];
396 |         double prev_neighb_weight = neighb_weights[prev_community];
397 |         community_map_[node_index] = -1;
398 |         community_size_[prev_community] -= prev_community_size[node_index];
399 |         community_in_weight_[prev_community] -= 2 * prev_neighb_weight + self_weight;
400 |         community_total_weight_[prev_community] -= total_weight;
401 | 
402 |         double max_inc = 0.0;
403 |         size_t best_community = prev_community;
404 |         double best_neighb_weight = prev_neighb_weight;
405 |         std::unordered_map<size_t, double>::const_iterator it = neighb_weights.begin();
406 |         for (; it != neighb_weights.end(); it++)
407 |         {
408 |             size_t neighb_community_index = it->first;
409 |             if (community_size_[neighb_community_index] >= max_community_)
410 |                 continue;
411 |             double neighb_weight = it->second;
412 |             double neighb_community_total_weight = community_total_weight_[neighb_community_index];
413 |             double inc = (neighb_weight - (neighb_community_total_weight * total_weight) / sum_edge_weight_) / sum_edge_weight_ * 2;
414 | 
415 |             if (inc > max_inc)
416 |             {
417 |                 max_inc = inc;
418 |                 best_community = neighb_community_index;
419 |                 best_neighb_weight = neighb_weight;
420 |             }
421 |         }
422 | 
423 |         community_map_[node_index] = best_community;
424 |         community_size_[best_community] += prev_community_size[node_index];
425 |         community_in_weight_[best_community] += 2 * best_neighb_weight  + self_weight;
426 |         community_total_weight_[best_community] += total_weight;
427 | 
428 |         if (best_community != prev_community)
429 |         {
430 |             for (size_t i = 0; i < edges.size(); i++)
431 |             {
432 |                 EdgeData const & edge = edges[i];
433 |                 size_t neighb_index = edge.node;
434 |                 size_t neighb_community_index = community_map_[neighb_index];
435 |                 if (visited[neighb_index] && community_size_[neighb_community_index] < max_community_)
436 |                 {
437 |                     queue.push(neighb_index);
438 |                     visited[neighb_index] = false;
439 |                 }
440 |             }
441 |             improved = true;
442 |         }
443 | 
444 |         if (++loop_count > 3 * node_num)
445 |             break;
446 |     }
447 |     return improved;
448 | }
449 | 
450 | bool Louvain::Merge(std::vector<std::pair<size_t, size_t> > const & initial_pairs)
451 | {
452 |     size_t node_num = graph_.NodeNum();
453 | 
454 |     std::vector<double> weights;
455 |     weights.reserve(initial_pairs.size());
456 |     for (size_t i = 0; i < initial_pairs.size(); i++)
457 |     {
458 |         size_t node_index1 = initial_pairs[i].first;
459 |         size_t node_index2 = initial_pairs[i].second;
460 |         double weight = graph_.EdgeWeight(node_index1, node_index2);
461 |         weights.push_back(weight);
462 |     }
463 |     std::vector<size_t> pair_indexes = SortIndexes(weights, false);
464 | 
465 |     UnionFind union_find;
466 |     union_find.InitSets(node_num);
467 | 
468 |     std::vector<size_t> node_visit_times;
469 |     node_visit_times.resize(node_num, 0);
470 |     for (size_t i = 0; i < pair_indexes.size(); i++)
471 |     {
472 |         size_t pair_index = pair_indexes[i];
473 |         size_t node_index1 = initial_pairs[pair_index].first;
474 |         size_t node_index2 = initial_pairs[pair_index].second;
475 |         if (node_visit_times[node_index1] < 1 && node_visit_times[node_index2] < 1)
476 |         {
477 |             union_find.Union(node_index1, node_index2);
478 |             node_visit_times[node_index1]++;
479 |             node_visit_times[node_index2]++;
480 |         }
481 |     }
482 | 
483 |     for (size_t i = 0; i < node_num; i++)
484 |     {
485 |         size_t node_index = i;
486 |         community_in_weight_[node_index] = 0.0;
487 |         community_total_weight_[node_index] = 0.0;
488 |         community_size_[node_index] = 0;
489 |     }
490 |     for (size_t i = 0; i < node_num; i++)
491 |     {
492 |         size_t node_index = i;
493 |         size_t community_index = union_find.Find(node_index);
494 |         community_map_[node_index] = community_index;
495 |         community_in_weight_[community_index] += graph_.GetSelfWeight(node_index);
496 |         community_total_weight_[community_index] += graph_.GetSelfWeight(node_index);
497 |         community_size_[community_index] += 1;
498 |     }
499 | 
500 |     for (size_t i = 0; i < node_num; i++)
501 |     {
502 |         size_t node_index1 = i;
503 |         size_t community_index1 = community_map_[node_index1];
504 |         std::vector<EdgeData> const & edges = graph_.GetIncidentEdges(node_index1);
505 |         for (size_t j = 0; j < edges.size(); j++)
506 |         {
507 |             EdgeData const & edge = edges[j];
508 |             size_t node_index2 = edge.node;
509 |             if (node_index1 > node_index2) continue;
510 |             size_t community_index2 = community_map_[node_index2];
511 |             double weight = edge.weight;
512 |             if (community_index1 == community_index2)
513 |             {
514 |                 community_in_weight_[community_index1] += 2 * weight;
515 |                 community_total_weight_[community_index1] += 2 * weight;
516 |             }
517 |             else
518 |             {
519 |                 community_total_weight_[community_index1] += weight;
520 |                 community_total_weight_[community_index2] += weight;
521 |             }
522 |         }
523 |     }
524 | 
525 |     return true;
526 | }
527 | 
528 | bool Louvain::StochasticMerge()
529 | {
530 |     bool improve = false;
531 |     std::queue<size_t> queue;
532 | 
533 |     size_t community_num = graph_.NodeNum();
534 |     for (size_t i = 0; i < community_num; i++)
535 |     {
536 |         size_t community_index = i;
537 |         if (community_size_[community_index] < max_community_)
538 |         {
539 |             queue.push(community_index);
540 |         }
541 |     }
542 | 
543 |     size_t node_num = node_graph_.NodeNum();
544 |     size_t node_edge_num = node_graph_.EdgeNum();
545 |     size_t community_edge_num = graph_.EdgeNum();
546 |     double factor = (community_num + community_edge_num) / double(node_num + node_edge_num);
547 | 
548 |     std::vector<size_t> prev_community_size = community_size_;
549 | 
550 |     while(!queue.empty())
551 |     {
552 |         size_t node_index = queue.front();
553 |         queue.pop();
554 | 
555 |         double self_weight = graph_.GetSelfWeight(node_index);
556 |         double total_weight = self_weight;
557 | 
558 |         std::unordered_map<size_t, double> neighb_weights;  // the weight of edge to neighboring community
559 |         std::vector<EdgeData> const & edges = graph_.GetIncidentEdges(node_index);
560 |         for (size_t i = 0; i < edges.size(); i++)
561 |         {
562 |             EdgeData const & edge = edges[i];
563 |             size_t neighb_index = edge.node;
564 |             size_t neighb_community_index = community_map_[neighb_index];
565 |             neighb_weights[neighb_community_index] += edge.weight;
566 |             total_weight += edge.weight;
567 |         }
568 | 
569 |         size_t prev_community = community_map_[node_index];
570 |         double prev_neighb_weight = neighb_weights[prev_community];
571 |         community_map_[node_index] = -1;
572 |         community_size_[prev_community] -= prev_community_size[node_index];
573 |         community_in_weight_[prev_community] -= 2 * prev_neighb_weight + self_weight;
574 |         community_total_weight_[prev_community] -= total_weight;
575 | 
576 |         std::unordered_map<size_t, double> prob_map;
577 |         std::unordered_map<size_t, double>::const_iterator it = neighb_weights.begin();
578 |         for (; it != neighb_weights.end(); it++)
579 |         {
580 |             size_t neighb_community_index = it->first;
581 |             size_t neighb_community_size = community_size_[neighb_community_index];
582 |             if (prev_community_size[node_index] + neighb_community_size >= max_community_)
583 |                 continue;
584 |             double neighb_weight = it->second;
585 |             double neighb_community_total_weight = community_total_weight_[neighb_community_index];
586 |             double inc = factor * (neighb_weight - (neighb_community_total_weight * total_weight) / sum_edge_weight_);
587 | 
588 |             double prob = std::exp(temperature_ * inc);
589 |             prob_map[neighb_community_index] = prob;
590 |         }
591 |         assert(!prob_map.empty());
592 | 
593 |         size_t best_community = RandomPick(prob_map);
594 |         double best_neighb_weight = neighb_weights[best_community];
595 |         if (best_community != prev_community)
596 |             improve = true;
597 | 
598 |         community_map_[node_index] = best_community;
599 |         community_size_[best_community] += prev_community_size[node_index];
600 |         community_in_weight_[best_community] += 2 * best_neighb_weight  + self_weight;
601 |         community_total_weight_[best_community] += total_weight;
602 |     }
603 |     return improve;
604 | }
605 | 
606 | void Louvain::RearrangeCommunities()
607 | {
608 |     std::unordered_map<size_t, size_t> renumbers;   // map from old cluster index to organized cluster index
609 | 
610 |     size_t num = 0;
611 |     for (size_t i = 0; i < community_map_.size(); i++)
612 |     {
613 |         std::unordered_map<size_t, size_t>::const_iterator it = renumbers.find(community_map_[i]);
614 |         if (it == renumbers.end())
615 |         {
616 |             renumbers[community_map_[i]] = num;
617 |             community_map_[i] = num;
618 |             num++;
619 |         }
620 |         else
621 |         {
622 |             community_map_[i] = it->second;
623 |         }
624 |     }
625 | 
626 |     for (size_t i = 0; i < node_map_.size(); i++)
627 |     {
628 |         node_map_[i] = community_map_[node_map_[i]];
629 |     }
630 | 
631 |     std::vector<size_t> community_size_new(num);
632 |     std::vector<double> community_in_weight_new(num);
633 |     std::vector<double> community_total_weight_new(num);
634 |     for (size_t i = 0; i < community_in_weight_.size(); i++)
635 |     {
636 |          std::unordered_map<size_t, size_t>::const_iterator it = renumbers.find(i);
637 |          if (it != renumbers.end())
638 |          {
639 |              size_t new_community_index = it->second;
640 |              community_size_new[new_community_index] = community_size_[i];
641 |              community_in_weight_new[new_community_index] = community_in_weight_[i];
642 |              community_total_weight_new[new_community_index] = community_total_weight_[i];
643 |          }
644 |     }
645 |     community_size_new.swap(community_size_);
646 |     community_in_weight_new.swap(community_in_weight_);
647 |     community_total_weight_new.swap(community_total_weight_);
648 | }
649 | 
650 | void Louvain::Rebuild()
651 | {
652 |     RearrangeCommunities();
653 | 
654 |     size_t num_communities = community_in_weight_.size();
655 |     std::vector<std::vector<size_t>> community_nodes(num_communities);
656 |     for (size_t i = 0; i < graph_.NodeNum(); i++)
657 |     {
658 |         community_nodes[community_map_[i]].push_back(i);
659 |     }
660 | 
661 |     Graph graph_new;
662 |     graph_new.Resize(num_communities);
663 |     for (size_t i = 0; i < num_communities; i++)
664 |     {
665 |         std::vector<size_t> const & nodes = community_nodes[i];
666 |         double self_weight = 0.0;
667 |         std::unordered_map<size_t, double> edges_new;
668 |         for (size_t j = 0; j < nodes.size(); j++)
669 |         {
670 |             size_t node_index = nodes[j];
671 |             self_weight += graph_.GetSelfWeight(node_index);
672 |             std::vector<EdgeData> const & edges = graph_.GetIncidentEdges(node_index);
673 |             for (size_t k = 0; k < edges.size(); k++)
674 |             {
675 |                 EdgeData const & edge = edges[k];
676 |                 edges_new[community_map_[edge.node]] += edge.weight;
677 |             }
678 |         }
679 |         self_weight += edges_new[i];
680 |         graph_new.AddSelfEdge(i, self_weight);
681 | 
682 |          std::unordered_map<size_t, double>::const_iterator it = edges_new.begin();
683 |          for (; it != edges_new.end(); it++)
684 |          {
685 |              size_t neighb_community_index = it->first;
686 |              double weight = it->second;
687 |              if (i != neighb_community_index)
688 |              {;
689 |                  graph_new.AddDirectedEdge(i, neighb_community_index, weight);
690 |              }
691 |          }
692 |     }
693 | 
694 |     graph_.Swap(graph_new);
695 | 
696 |     community_map_.resize(num_communities);
697 |     for (size_t i = 0; i < num_communities; i++)
698 |     {
699 |         community_map_[i] = i;
700 |     }
701 | }
702 | 


--------------------------------------------------------------------------------
/src/stochasticbaproblem.cpp:
--------------------------------------------------------------------------------
   1 | #include "STBA/stochasticbaproblem.h"
   2 | 
   3 | #include <fstream>
   4 | #include <numeric>
   5 | 
   6 | StochasticBAProblem::StochasticBAProblem() : LMBAProblem(), cluster_(NULL), batch_size_(1), inner_step_(4), complementary_clustering_(true)
   7 | {
   8 |     cluster_ = new Louvain();
   9 |     cluster_->SetMaxCommunity(100);
  10 |     cluster_->SetTemperature(10);
  11 | }
  12 | 
  13 | StochasticBAProblem::StochasticBAProblem(size_t max_iter,
  14 |                                          double radius,
  15 |                                          LossType loss_type,
  16 |                                          size_t max_community,
  17 |                                          size_t inner_step)
  18 |     : LMBAProblem(max_iter, radius, loss_type), cluster_(NULL), batch_size_(1), inner_step_(inner_step), complementary_clustering_(true)
  19 | {
  20 |     cluster_ = new Louvain();
  21 |     cluster_->SetMaxCommunity(max_community);
  22 |     cluster_->SetTemperature(10);
  23 | }
  24 | 
  25 | StochasticBAProblem::StochasticBAProblem(size_t max_iter,
  26 |                                          double radius,
  27 |                                          LossType loss_type,
  28 |                                          size_t max_community,
  29 |                                          double temperature,
  30 |                                          size_t batch_size,
  31 |                                          size_t inner_step,
  32 |                                          bool complementary_clustering)
  33 |     : LMBAProblem(max_iter, radius, loss_type), cluster_(NULL), batch_size_(batch_size), inner_step_(inner_step), complementary_clustering_(complementary_clustering)
  34 | {
  35 |     cluster_ = new Louvain();
  36 |     cluster_->SetMaxCommunity(max_community);
  37 |     cluster_->SetTemperature(temperature);
  38 | }
  39 | 
  40 | StochasticBAProblem::StochasticBAProblem(size_t pose_num, size_t group_num, size_t point_num, size_t proj_num) : LMBAProblem(pose_num, group_num, point_num, proj_num), cluster_(NULL), batch_size_(1), inner_step_(0)
  41 | {
  42 |     cluster_ = new Louvain();
  43 |     cluster_->SetMaxCommunity(100);
  44 | }
  45 | 
  46 | StochasticBAProblem::~StochasticBAProblem()
  47 | {
  48 |     if (cluster_ != NULL)
  49 |         delete cluster_;
  50 | }
  51 | 
  52 | void StochasticBAProblem::Solve()
  53 | {
  54 |     std::cout << "[Solve] max_community = " << cluster_->GetMaxCommunity() << "\n"
  55 |               << "[Solve] temperature = " << cluster_->GetTemperature() << "\n"
  56 |               << "[Solve] batch size = " << batch_size_ << "\n"
  57 |               << "[Solve] inner step = " << inner_step_ << "\n"
  58 |               << "[Solve] complementary clustering = " << complementary_clustering_ << "\n"
  59 |               << "[Solve] thread number = " << thread_num_ << "\n";
  60 | 
  61 |     last_square_error_ = EvaluateSquareError(false);
  62 |     double mean_error, median_error, max_error;
  63 |     ReprojectionError(mean_error, median_error, max_error, false);
  64 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
  65 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
  66 |     evaluate_ = true;
  67 |     cluster_->Cluster();
  68 | 
  69 |     time_ = std::chrono::system_clock::now();
  70 |     for (iter_ = 0; iter_ < max_iteration_; iter_++)
  71 |     {
  72 |         ClearUpdate();
  73 |         if (evaluate_)
  74 |         {
  75 |             EvaluateResidual();
  76 |             EvaluateJacobian();
  77 |             EvaluateJcJc();
  78 |             EvaluateJcJp();
  79 |             EvaluateJce();
  80 |             BAProblem::EvaluateJpJp();
  81 |             BAProblem::EvaluateJpe();
  82 |         }
  83 | 
  84 |         LMBAProblem::AugmentPoseDiagonal();
  85 |         LMBAProblem::AugmentPointDiagonal();
  86 | 
  87 |         if (EvaluateDeltaPose())
  88 |         {
  89 |             EvaluateDeltaPoint();
  90 |             square_error_ = EvaluateSquareError(true);
  91 |             if (StopCriterionGradient() || StopCriterionUpdate() || StopCriterionRadius() || StopCriterionRelativeCostChange())
  92 |                 break;
  93 |             step_accept_ = StepAccept();
  94 |         }
  95 |         else
  96 |         {
  97 |             std::cout << "Fail in EvaluateDeltaPose.\n";
  98 |             step_accept_ = false;
  99 |         }
 100 | 
 101 |         if (step_accept_) // accept, descrease lambda
 102 |         {
 103 |             Print();
 104 |             IncreaseRadius();
 105 |             last_square_error_ = square_error_;
 106 |             UpdateParam();
 107 |             evaluate_ = true;
 108 |         }
 109 |         else // reject, increase lambda
 110 |         {
 111 |             Print();
 112 |             DecreaseRadius();
 113 |             LMBAProblem::ResetPoseDiagonal();
 114 |             LMBAProblem::ResetPointDiagonal();
 115 |             evaluate_ = false;
 116 |         }
 117 |     }
 118 | 
 119 |     std::cout << "[Solve] Before: mean / median / max reprojection error = "
 120 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
 121 |     stream_ << "[Solve] Before: mean / median / max reprojection error = "
 122 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
 123 |     ReprojectionError(mean_error, median_error, max_error, false);
 124 |     std::cout << "[Solve] After: mean / median / max reprojection error = "
 125 |               << mean_error << " / " << median_error << " / " << max_error << "\n";
 126 |     stream_ << "[Solve] After: mean / median / max reprojection error = "
 127 |             << mean_error << " / " << median_error << " / " << max_error << "\n";
 128 |     stream_ << "[Setting] max_community = " << cluster_->GetMaxCommunity() << "\n"
 129 |             << "[Setting] temperature = " << cluster_->GetTemperature() << "\n"
 130 |             << "[Setting] batch size = " << batch_size_ << "\n"
 131 |             << "[Setting] inner step = " << inner_step_ << "\n"
 132 |             << "[Setting] complementary clustering = " << complementary_clustering_ << "\n"
 133 |             << "[Setting] thread number = " << thread_num_ << "\n"
 134 |             << "[Setting] STBA\n";
 135 | }
 136 | 
 137 | size_t StochasticBAProblem::GetPoseCluster(size_t pose_index) const
 138 | {
 139 |     assert(pose_index < PoseNum() && "[GetPoseCluster] Pose index not found");
 140 |     return pose_cluster_map_[pose_index];
 141 | }
 142 | 
 143 | size_t StochasticBAProblem::GetPointLocalCluster(size_t point_index, size_t cluster_index) const
 144 | {
 145 |     size_t local_point_cluster;
 146 |     bool found = false;
 147 |     std::vector<size_t> const &point_clusters = point_cluster_map_[point_index];
 148 |     for (size_t j = 0; j < point_clusters.size(); j++)
 149 |     {
 150 |         if (point_clusters[j] == cluster_index)
 151 |         {
 152 |             local_point_cluster = j;
 153 |             found = true;
 154 |             break;
 155 |         }
 156 |     }
 157 |     assert(found && "[GetPointLocalCluster] Point cluster not found");
 158 |     return local_point_cluster;
 159 | }
 160 | 
 161 | size_t StochasticBAProblem::GetPointClusters(size_t point_index, std::vector<size_t> &cluster_indexes) const
 162 | {
 163 |     std::unordered_set<size_t> cluster_set;
 164 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = point_projection_map_.find(point_index);
 165 |     assert(it1 != point_projection_map_.end() && "[GetPointClusters] Point index not found");
 166 |     std::unordered_map<size_t, size_t> const &map = it1->second;
 167 |     std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 168 |     for (; it2 != map.end(); it2++)
 169 |     {
 170 |         size_t pose_index = it2->first;
 171 |         size_t cluster_index = GetPoseCluster(pose_index);
 172 |         cluster_set.insert(cluster_index);
 173 |     }
 174 |     cluster_indexes = std::vector<size_t>(cluster_set.begin(), cluster_set.end());
 175 |     return cluster_indexes.size();
 176 | }
 177 | 
 178 | void StochasticBAProblem::AugmentPointDiagonal()
 179 | {
 180 |     std::vector<std::vector<Vec3>> aug_point_diagonal;
 181 |     GetPointDiagonal(cluster_point_diagonal_);
 182 |     aug_point_diagonal = cluster_point_diagonal_;
 183 |     GetPointAugDiagonal(aug_point_diagonal);
 184 |     AddPointDiagonal(aug_point_diagonal);
 185 | }
 186 | 
 187 | void StochasticBAProblem::ResetPointDiagonal()
 188 | {
 189 |     SetPointDiagonal(cluster_point_diagonal_);
 190 | }
 191 | 
 192 | void StochasticBAProblem::GetJpJp(size_t point_index, size_t local_cluster_index, Mat3 &JpJp) const
 193 | {
 194 |     assert(point_index < PointNum() && "[GetJpJp] Point index out of range");
 195 |     PointMeta const *point_ = point_meta_[point_index];
 196 |     point_->GetJpJp(local_cluster_index, JpJp);
 197 | }
 198 | 
 199 | void StochasticBAProblem::SetJpJp(size_t point_index, size_t local_cluster_index, Mat3 const &JpJp)
 200 | {
 201 |     assert(point_index < PointNum() && "[SetJpJp] Point index out of range");
 202 |     PointMeta *point_ = point_meta_[point_index];
 203 |     point_->SetJpJp(local_cluster_index, JpJp);
 204 | }
 205 | 
 206 | void StochasticBAProblem::GetJpe(size_t point_index, size_t local_cluster_index, Vec3 &Jpe) const
 207 | {
 208 |     assert(point_index < PointNum() && "[GetJpe] Point index out of range");
 209 |     PointMeta const *point_ = point_meta_[point_index];
 210 |     point_->GetJpe(local_cluster_index, Jpe);
 211 | }
 212 | 
 213 | void StochasticBAProblem::SetJpe(size_t point_index, size_t local_cluster_index, Vec3 const &Jpe)
 214 | {
 215 |     assert(point_index < PointNum() && "[SetJpe] Point index out of range");
 216 |     PointMeta *point_ = point_meta_[point_index];
 217 |     point_->SetJpe(local_cluster_index, Jpe);
 218 | }
 219 | 
 220 | void StochasticBAProblem::GetDeltaPoint(size_t point_index, size_t local_cluster_index, Vec3 &dz) const
 221 | {
 222 |     assert(point_index < PointNum() && "[Getdz] Point index out of range");
 223 |     PointMeta const *point_ = point_meta_[point_index];
 224 |     point_->GetDeltaPoint(local_cluster_index, dz);
 225 | }
 226 | 
 227 | void StochasticBAProblem::SetDeltaPoint(size_t point_index, size_t local_cluster_index, Vec3 const &dz)
 228 | {
 229 |     assert(point_index < PointNum() && "[Setdz] Point index out of range");
 230 |     PointMeta *point_ = point_meta_[point_index];
 231 |     point_->SetDeltaPoint(local_cluster_index, dz);
 232 | }
 233 | 
 234 | void StochasticBAProblem::ClearPointMeta()
 235 | {
 236 |     size_t point_num = point_meta_.size();
 237 |     for (size_t i = 0; i < point_num; i++)
 238 |     {
 239 |         PointMeta *ptr = point_meta_[i];
 240 |         if (ptr != NULL)
 241 |             delete ptr;
 242 |     }
 243 |     point_meta_.clear();
 244 | }
 245 | 
 246 | void StochasticBAProblem::GetPointDiagonal(std::vector<std::vector<Vec3>> &point_diagonal) const
 247 | {
 248 |     size_t point_num = PointNum();
 249 |     point_diagonal.resize(point_num);
 250 |     for (size_t i = 0; i < point_num; i++)
 251 |     {
 252 |         PointMeta const *point_ = point_meta_[i];
 253 |         std::vector<size_t> const &clusters = point_cluster_map_[i];
 254 |         std::vector<Vec3> &cluster_diagonal = point_diagonal[i];
 255 |         cluster_diagonal.resize(clusters.size());
 256 |         for (size_t j = 0; j < clusters.size(); j++)
 257 |         {
 258 |             Vec3 diagonal;
 259 |             point_->GetDiagonal(j, diagonal);
 260 |             cluster_diagonal[j] = diagonal;
 261 |         }
 262 |     }
 263 | }
 264 | 
 265 | void StochasticBAProblem::GetPointAugDiagonal(std::vector<std::vector<Vec3>> &aug_point_diagonal) const
 266 | {
 267 |     size_t point_num = PointNum();
 268 |     for (size_t i = 0; i < point_num; i++)
 269 |     {
 270 |         std::vector<Vec3> &diagonals = aug_point_diagonal[i];
 271 |         for (size_t j = 0; j < diagonals.size(); j++)
 272 |         {
 273 |             Vec3 &diagonal = diagonals[j];
 274 |             diagonal = diagonal / mu_;
 275 |         }
 276 |     }
 277 | }
 278 | 
 279 | void StochasticBAProblem::AddPointDiagonal(std::vector<std::vector<Vec3>> const &aug_point_diagonal)
 280 | {
 281 |     size_t point_num = PointNum();
 282 |     for (size_t i = 0; i < point_num; i++)
 283 |     {
 284 |         PointMeta *point_ = point_meta_[i];
 285 |         std::vector<Vec3> const &diagonals = aug_point_diagonal[i];
 286 |         for (size_t j = 0; j < diagonals.size(); j++)
 287 |         {
 288 |             Vec3 diagonal = diagonals[j];
 289 |             point_->AddDiagonal(j, diagonal);
 290 |         }
 291 |     }
 292 | }
 293 | 
 294 | void StochasticBAProblem::SetPointDiagonal(std::vector<std::vector<Vec3>> const &point_diagonal)
 295 | {
 296 |     size_t point_num = PointNum();
 297 |     for (size_t i = 0; i < point_num; i++)
 298 |     {
 299 |         PointMeta *point_ = point_meta_[i];
 300 |         std::vector<Vec3> const &diagonals = point_diagonal[i];
 301 |         for (size_t j = 0; j < diagonals.size(); j++)
 302 |         {
 303 |             Vec3 diagonal = diagonals[j];
 304 |             point_->SetDiagonal(j, diagonal);
 305 |         }
 306 |     }
 307 | }
 308 | 
 309 | void StochasticBAProblem::EvaluateJpJp(size_t point_index, size_t cluster_index, Mat3 &JpJp) const
 310 | {
 311 |     JpJp = Mat3::Zero();
 312 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = point_projection_map_.find(point_index);
 313 |     assert(it1 != point_projection_map_.end() && "[GetJpJp] Point index not found");
 314 |     std::unordered_map<size_t, size_t> const &map = it1->second;
 315 |     std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 316 |     for (; it2 != map.end(); it2++)
 317 |     {
 318 |         size_t pose_index = it2->first;
 319 |         size_t local_cluster_index = GetPoseCluster(pose_index);
 320 |         if (cluster_index != local_cluster_index)
 321 |             continue;
 322 |         size_t proj_index = it2->second;
 323 |         Mat23 jacobian;
 324 |         GetPointJacobian(proj_index, jacobian);
 325 |         JpJp += jacobian.transpose() * jacobian;
 326 |     }
 327 | }
 328 | 
 329 | void StochasticBAProblem::EvaluateJpJp()
 330 | {
 331 |     size_t point_num = point_block_.PointNum();
 332 | 
 333 | #pragma omp parallel for
 334 |     for (size_t i = 0; i < point_num; i++)
 335 |     {
 336 |         std::vector<size_t> const &cluster_indexes = point_cluster_map_[i];
 337 |         for (size_t j = 0; j < cluster_indexes.size(); j++)
 338 |         {
 339 |             size_t cluster_index = cluster_indexes[j];
 340 |             Mat3 jpjp;
 341 |             EvaluateJpJp(i, cluster_index, jpjp);
 342 |             SetJpJp(i, j, jpjp);
 343 |         }
 344 |     }
 345 | }
 346 | 
 347 | void StochasticBAProblem::EvaluateJpe(size_t point_index, size_t cluster_index, Vec3 &Jpe) const
 348 | {
 349 |     Jpe = Vec3::Zero();
 350 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = point_projection_map_.find(point_index);
 351 |     assert(it1 != point_projection_map_.end() && "[GetJpe] Point index not found");
 352 |     std::unordered_map<size_t, size_t> const &map = it1->second;
 353 |     std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 354 |     for (; it2 != map.end(); it2++)
 355 |     {
 356 |         size_t pose_index = it2->first;
 357 |         size_t local_cluster_index = GetPoseCluster(pose_index);
 358 |         if (cluster_index != local_cluster_index)
 359 |             continue;
 360 | 
 361 |         size_t proj_index = it2->second;
 362 |         Mat23 point_jacobian;
 363 |         GetPointJacobian(proj_index, point_jacobian);
 364 | 
 365 |         Vec2 residual;
 366 |         GetResidual(proj_index, residual);
 367 |         Jpe += point_jacobian.transpose() * residual;
 368 |     }
 369 | }
 370 | 
 371 | void StochasticBAProblem::EvaluateJpe()
 372 | {
 373 |     size_t point_num = point_block_.PointNum();
 374 | 
 375 | #pragma omp parallel for
 376 |     for (size_t i = 0; i < point_num; i++)
 377 |     {
 378 |         std::vector<size_t> const &cluster_indexes = point_cluster_map_[i];
 379 |         for (size_t j = 0; j < cluster_indexes.size(); j++)
 380 |         {
 381 |             size_t cluster_index = cluster_indexes[j];
 382 |             Vec3 jpe;
 383 |             EvaluateJpe(i, cluster_index, jpe);
 384 |             SetJpe(i, j, jpe);
 385 |         }
 386 |     }
 387 | }
 388 | 
 389 | bool StochasticBAProblem::EvaluateEcEc(size_t pose_index1, size_t pose_index2, Mat6 &EcEc) const
 390 | {
 391 |     EcEc.setZero();
 392 |     std::vector<size_t> points;
 393 |     GetCommonPoints(pose_index1, pose_index2, points);
 394 |     if (points.empty())
 395 |         return false;
 396 | 
 397 |     size_t cluster_index1 = GetPoseCluster(pose_index1);
 398 |     size_t cluster_index2 = GetPoseCluster(pose_index2);
 399 |     assert(cluster_index1 == cluster_index2 && "[EvaluateEcEc] Clusters of two poses disagree");
 400 | 
 401 |     for (size_t i = 0; i < points.size(); i++)
 402 |     {
 403 |         size_t point_index = points[i];
 404 |         size_t local_point_cluster = GetPointLocalCluster(point_index, cluster_index1);
 405 | 
 406 |         Mat63 Jc1Jp, Jc2Jp;
 407 |         Mat3 JpJp;
 408 |         GetJcJp(pose_index1, point_index, Jc1Jp);
 409 |         GetJcJp(pose_index2, point_index, Jc2Jp);
 410 |         GetJpJp(point_index, local_point_cluster, JpJp);
 411 |         Mat3 JpJp_inv = JpJp.inverse();
 412 |         if (IsNumericalValid(JpJp_inv))
 413 |             EcEc += Jc1Jp * JpJp_inv * Jc2Jp.transpose();
 414 |     }
 415 |     return true;
 416 | }
 417 | 
 418 | void StochasticBAProblem::EvaluateEcEc(std::vector<size_t> const &pose_indexes, MatX &EcEc) const
 419 | {
 420 |     size_t pose_num = pose_indexes.size();
 421 |     EcEc = MatX::Zero(pose_num * 6, pose_num * 6);
 422 | 
 423 |     if (pose_indexes.empty())
 424 |         return;
 425 | 
 426 |     size_t cluster_index = GetPoseCluster(pose_indexes[0]);
 427 |     std::unordered_map<size_t, size_t> local_pose_map;
 428 |     for (size_t i = 0; i < pose_indexes.size(); i++)
 429 |     {
 430 |         size_t pose_index = pose_indexes[i];
 431 |         local_pose_map[pose_index] = i;
 432 |     }
 433 | 
 434 |     std::vector<size_t> const &points = cluster_points_[cluster_index];
 435 |     for (size_t i = 0; i < points.size(); i++)
 436 |     {
 437 |         size_t point_index = points[i];
 438 |         std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = point_projection_map_.find(point_index);
 439 |         assert(it1 != point_projection_map_.end() && "[EvaluateEcEc] Point index not found");
 440 |         std::unordered_map<size_t, size_t> const &map = it1->second;
 441 |         std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 442 |         std::vector<size_t> cluster_pose_indexes, cluster_proj_indexes;
 443 |         for (; it2 != map.end(); it2++)
 444 |         {
 445 |             size_t pose_index = it2->first;
 446 |             size_t proj_index = it2->second;
 447 |             size_t local_cluster_index = GetPoseCluster(pose_index);
 448 |             if (local_cluster_index == cluster_index)
 449 |             {
 450 |                 cluster_pose_indexes.push_back(pose_index);
 451 |                 cluster_proj_indexes.push_back(proj_index);
 452 |             }
 453 |         }
 454 |         Mat3 JpJp, JpJp_inv;
 455 |         GetJpJp(point_index, GetPointLocalCluster(point_index, cluster_index), JpJp);
 456 |         JpJp_inv = JpJp.inverse();
 457 |         if (IsNumericalValid(JpJp_inv))
 458 |         {
 459 |             for (size_t j = 0; j < cluster_pose_indexes.size(); j++)
 460 |             {
 461 |                 size_t pose_index1 = cluster_pose_indexes[j];
 462 |                 size_t proj_index1 = cluster_proj_indexes[j];
 463 |                 assert(local_pose_map.find(pose_index1) != local_pose_map.end());
 464 |                 size_t local_pose_index1 = local_pose_map[pose_index1];
 465 |                 Mat63 Jc1Jp;
 466 |                 GetJcJp(proj_index1, Jc1Jp);
 467 |                 for (size_t k = j; k < cluster_pose_indexes.size(); k++)
 468 |                 {
 469 |                     size_t pose_index2 = cluster_pose_indexes[k];
 470 |                     size_t proj_index2 = cluster_proj_indexes[k];
 471 |                     assert(local_pose_map.find(pose_index2) != local_pose_map.end());
 472 |                     size_t local_pose_index2 = local_pose_map[pose_index2];
 473 |                     Mat63 Jc2Jp;
 474 |                     GetJcJp(proj_index2, Jc2Jp);
 475 |                     Mat6 ece = Jc1Jp * JpJp_inv * Jc2Jp.transpose();
 476 |                     EcEc.block(local_pose_index1 * 6, local_pose_index2 * 6, 6, 6) += ece;
 477 |                     if (pose_index1 != pose_index2)
 478 |                     {
 479 |                         EcEc.block(local_pose_index2 * 6, local_pose_index1 * 6, 6, 6) += ece.transpose();
 480 |                     }
 481 |                 }
 482 |             }
 483 |         }
 484 |     }
 485 | }
 486 | 
 487 | void StochasticBAProblem::EvaluateEDeltaPose(size_t point_index, size_t cluster_index, Vec3 &Edy) const
 488 | {
 489 |     Edy = Vec3::Zero();
 490 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = point_projection_map_.find(point_index);
 491 |     assert(it1 != point_projection_map_.end() && "[EvaluateEDeltaPose] Point index not found");
 492 |     std::unordered_map<size_t, size_t> const &map = it1->second;
 493 |     std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 494 |     for (; it2 != map.end(); it2++)
 495 |     {
 496 |         size_t pose_index = it2->first;
 497 |         if (GetPoseCluster(pose_index) != cluster_index)
 498 |             continue;
 499 |         size_t proj_index = it2->second;
 500 |         Mat63 JcJp;
 501 |         Vec6 dy;
 502 |         GetJcJp(proj_index, JcJp);
 503 |         pose_block_.GetTempDeltaPose(pose_index, dy);
 504 |         Edy += JcJp.transpose() * dy;
 505 |     }
 506 | }
 507 | 
 508 | /*!
 509 |  * @brief S dy = b, omitting intrinsic blocks here
 510 |  */
 511 | bool StochasticBAProblem::EvaluateDeltaPose(std::vector<size_t> const &pose_indexes, VecX const &b, VecX &dy) const
 512 | {
 513 |     bool ret;
 514 |     if (pose_indexes.size() < 5000)
 515 |     {
 516 |         MatX S;
 517 |         BAProblem::EvaluateSchurComplement(pose_indexes, S);
 518 |         ret = SolveLinearSystem(S, b, dy);
 519 |     }
 520 |     else
 521 |     {
 522 |         SMat S;
 523 |         BAProblem::EvaluateSchurComplement(pose_indexes, S);
 524 |         ret = SolveLinearSystem(S, b, dy);
 525 |     }
 526 | 
 527 |     return ret;
 528 | }
 529 | 
 530 | void StochasticBAProblem::EvaluateSchurComplement(std::vector<std::unordered_map<size_t, Mat6>> &S) const
 531 | {
 532 |     size_t pose_num = PoseNum();
 533 |     S.clear();
 534 |     S.resize(pose_num);
 535 | 
 536 |     std::vector<std::pair<size_t, size_t>> pose_pairs;
 537 |     std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t>>>::const_iterator it1 = common_point_map_.begin();
 538 |     for (; it1 != common_point_map_.end(); it1++)
 539 |     {
 540 |         size_t pose_index1 = it1->first;
 541 |         std::unordered_map<size_t, std::vector<size_t>> const &map = it1->second;
 542 |         std::unordered_map<size_t, std::vector<size_t>>::const_iterator it2 = map.begin();
 543 |         for (; it2 != map.end(); it2++)
 544 |         {
 545 |             size_t pose_index2 = it2->first;
 546 |             pose_pairs.push_back(std::make_pair(pose_index1, pose_index2));
 547 |             if (pose_index1 == pose_index2)
 548 |             {
 549 |                 S[pose_index1][pose_index1] = Mat6::Zero();
 550 |             }
 551 |             else
 552 |             {
 553 |                 S[pose_index1][pose_index2] = Mat6::Zero();
 554 |                 S[pose_index2][pose_index1] = Mat6::Zero();
 555 |             }
 556 |         }
 557 |     }
 558 | 
 559 |     for (size_t i = 0; i < pose_pairs.size(); i++)
 560 |     {
 561 |         size_t pose_index1 = pose_pairs[i].first;
 562 |         size_t pose_index2 = pose_pairs[i].second;
 563 |         Mat6 local_EcEc;
 564 |         bool ret = BAProblem::EvaluateEcEc(pose_index1, pose_index2, local_EcEc);
 565 |         if (ret)
 566 |         {
 567 |             if (pose_index1 == pose_index2)
 568 |             {
 569 |                 Mat6 JcJc;
 570 |                 GetJcJc(pose_index1, JcJc);
 571 |                 S[pose_index1][pose_index1] = JcJc - local_EcEc;
 572 |             }
 573 |             else
 574 |             {
 575 |                 S[pose_index1][pose_index2] = -local_EcEc;
 576 |                 S[pose_index2][pose_index1] = -local_EcEc.transpose();
 577 |             }
 578 |         }
 579 |     }
 580 | }
 581 | 
 582 | void StochasticBAProblem::EvaluateSdy(std::vector<std::unordered_map<size_t, Mat6>> const &S,
 583 |                                       VecX const &dy, VecX &Sdy) const
 584 | {
 585 |     size_t pose_num = PoseNum();
 586 |     assert(S.size() == pose_num);
 587 |     assert(dy.rows() == pose_num * 6);
 588 |     Sdy = VecX::Zero(pose_num * 6);
 589 | 
 590 | #pragma omp parallel for
 591 |     for (size_t i = 0; i < pose_num; i++)
 592 |     {
 593 |         Vec6 local_Sdy = Vec6::Zero();
 594 |         std::unordered_map<size_t, Mat6> const &local_S = S[i];
 595 |         std::unordered_map<size_t, Mat6>::const_iterator it = local_S.begin();
 596 |         for (; it != local_S.end(); it++)
 597 |         {
 598 |             size_t pose_index = it->first;
 599 |             Mat6 const &block = it->second;
 600 |             Vec6 local_dy = dy.segment(pose_index * 6, 6);
 601 |             local_Sdy += block * local_dy;
 602 |         }
 603 |         Sdy.segment(6 * i, 6) = local_Sdy;
 604 |     }
 605 | }
 606 | 
 607 | void StochasticBAProblem::InnerIteration(VecX &dy) const
 608 | {
 609 |     for (size_t i = 0; i < inner_step_; i++)
 610 |     {
 611 |         VecX Sdy;
 612 |         EvaluateSdy(full_S_, dy, Sdy);
 613 |         VecX residual_b = full_b_ - Sdy;
 614 | 
 615 |         for (size_t j = 0; j < PoseNum(); j++)
 616 |         {
 617 |             Mat6 JcJc, JcJc_inv;
 618 |             GetJcJc(j, JcJc);
 619 |             JcJc_inv = JcJc.inverse();
 620 |             if (IsNumericalValid(JcJc_inv))
 621 |             {
 622 |                 Vec6 local_dy = JcJc_inv * residual_b.segment(6 * j, 6);
 623 |                 dy.segment(j * 6, 6) += local_dy;
 624 |             }
 625 |         }
 626 |     }
 627 | }
 628 | 
 629 | void StochasticBAProblem::EvaluateFullb(VecX &b) const
 630 | {
 631 |     VecX Jce, ecw;
 632 |     GetJce(Jce);
 633 |     ecw.resize(PoseNum() * 6);
 634 | 
 635 | #pragma omp parallel for
 636 |     for (size_t i = 0; i < PoseNum(); i++)
 637 |     {
 638 |         Vec6 local_ecw;
 639 |         BAProblem::EvaluateEcw(i, local_ecw);
 640 |         ecw.segment(6 * i, 6) = local_ecw;
 641 |     }
 642 |     b = -Jce - ecw;
 643 | }
 644 | 
 645 | bool StochasticBAProblem::EvaluateDeltaPose()
 646 | {
 647 |     VecX dy = VecX::Zero(PoseNum() * 6);
 648 |     if (inner_step_ > 0)
 649 |     {
 650 |         EvaluateSchurComplement(full_S_);
 651 |         EvaluateFullb(full_b_);
 652 |     }
 653 | 
 654 |     // Compute initial steps
 655 |     RunCluster();
 656 |     EvaluateJpJp();
 657 |     AugmentPointDiagonal();
 658 |     EvaluateJpe();
 659 |     if (mu_ <= 1.0)
 660 |         SteepestDescentCorrection();
 661 |     BAProblem::EvaluateEcw();
 662 | 
 663 |     size_t cluster_num = cluster_poses_.size();
 664 |     size_t sum_broken = 0;
 665 | 
 666 | #pragma omp parallel for reduction(+: sum_broken)
 667 |     for (size_t i = 0; i < cluster_num; i++)
 668 |     {
 669 |         std::vector<size_t> const &pose_cluster = cluster_poses_[i];
 670 |         VecX cluster_dy;
 671 |         if (!BAProblem::EvaluateDeltaPose(pose_cluster, cluster_dy))
 672 |         {
 673 |             std::cout << "[EvaluateDeltaPose] Fail in solver linear system.\n";
 674 |             sum_broken += 1;
 675 |         }
 676 |         for (size_t j = 0; j < pose_cluster.size(); j++)
 677 |         {
 678 |             size_t pose_index = pose_cluster[j];
 679 |             dy.segment(pose_index * 6, 6) += cluster_dy.segment(j * 6, 6);
 680 |         }
 681 |     }
 682 |     if (sum_broken != 0)
 683 |     {
 684 |         std::cout << "[StochasticBAProblem::EvaluateDeltaPose] Fail in computing initial step.\n";
 685 |         return false;
 686 |     }
 687 | 
 688 |     InnerIteration(dy);
 689 | 
 690 |     for (size_t i = 0; i < PoseNum(); i++)
 691 |     {
 692 |         Vec3 angle_axis = dy.segment(i * 6, 3);
 693 |         Vec3 translation = dy.segment(i * 6 + 3, 3);
 694 |         pose_block_.SetDeltaPose(i, angle_axis, translation);
 695 |     }
 696 | 
 697 |     return true;
 698 | }
 699 | 
 700 | void StochasticBAProblem::EvaluateDeltaPoint(size_t point_index, Vec3 &dz)
 701 | {
 702 |     dz = Vec3::Zero();
 703 |     Mat3 sum_JpJp = Mat3::Zero();
 704 | 
 705 |     std::vector<size_t> const &cluster_indexes = point_cluster_map_[point_index];
 706 |     size_t cluster_num = cluster_indexes.size();
 707 |     assert(cluster_num > 0 && "[EvaluateDeltaPoint] Zero cluster number");
 708 | 
 709 |     size_t valid_cluster_num = 0;
 710 |     for (size_t i = 0; i < cluster_num; i++)
 711 |     {
 712 |         size_t cluster_index = cluster_indexes[i];
 713 |         Vec3 Jpe, Edy;
 714 |         Mat3 JpJp, JpJp_inv;
 715 |         GetJpe(point_index, i, Jpe);
 716 |         EvaluateEDeltaPose(point_index, cluster_index, Edy);
 717 |         GetJpJp(point_index, i, JpJp);
 718 |         Vec3 delta = -Jpe - Edy;
 719 |         dz += delta;
 720 |         sum_JpJp += JpJp;
 721 |         valid_cluster_num++;
 722 | 
 723 |         JpJp_inv = JpJp.inverse();
 724 |         if (IsNumericalValid(JpJp_inv))
 725 |         {
 726 |             Vec3 cluster_delta = JpJp_inv * delta;
 727 |             SetDeltaPoint(point_index, i, cluster_delta);
 728 |         }
 729 |     }
 730 | 
 731 |     if (valid_cluster_num > 0)
 732 |     {
 733 |         Mat3 sum_JpJp_inv = sum_JpJp.inverse();
 734 |         if (IsNumericalValid(sum_JpJp_inv))
 735 |             dz = sum_JpJp_inv * dz;
 736 |     }
 737 | }
 738 | 
 739 | void StochasticBAProblem::EvaluateDeltaPoint()
 740 | {
 741 |     size_t point_num = PointNum();
 742 | 
 743 | #pragma omp parallel for
 744 |     for (size_t i = 0; i < point_num; i++)
 745 |     {
 746 |         Vec3 dz;
 747 |         BAProblem::EvaluateDeltaPoint(i, dz);
 748 |         point_block_.SetDeltaPoint(i, dz);
 749 |     }
 750 | }
 751 | 
 752 | void StochasticBAProblem::EvaluateEcw(size_t pose_index, Vec6 &Ecw) const
 753 | {
 754 |     Ecw = Vec6::Zero();
 755 |     size_t pose_cluster_index = GetPoseCluster(pose_index);
 756 |     std::unordered_map<size_t, std::unordered_map<size_t, size_t>>::const_iterator it1 = pose_projection_map_.find(pose_index);
 757 |     assert(it1 != pose_projection_map_.end() && "[EvaluateECw] Pose index not found");
 758 |     std::unordered_map<size_t, size_t> const &map = it1->second;
 759 |     std::unordered_map<size_t, size_t>::const_iterator it2 = map.begin();
 760 |     for (; it2 != map.end(); it2++)
 761 |     {
 762 |         size_t point_index = it2->first;
 763 |         size_t proj_index = it2->second;
 764 |         size_t local_point_cluster = GetPointLocalCluster(point_index, pose_cluster_index);
 765 | 
 766 |         Mat63 JcJp;
 767 |         Mat3 JpJp;
 768 |         Vec3 Jpe;
 769 |         GetJcJp(proj_index, JcJp);
 770 |         GetJpJp(point_index, local_point_cluster, JpJp);
 771 |         GetJpe(point_index, local_point_cluster, Jpe);
 772 |         Mat3 JpJp_inv = JpJp.inverse();
 773 |         if (IsNumericalValid(JpJp_inv))
 774 |         {
 775 |             Ecw += JcJp * JpJp_inv * (-Jpe);
 776 |         }
 777 |     }
 778 | }
 779 | 
 780 | double StochasticBAProblem::EvaluateRSquare(VecX const &aug_pose_diagonal,
 781 |                                             std::vector<std::vector<Vec3>> const &aug_point_diagonal) const
 782 | {
 783 |     double R = 0;
 784 |     size_t proj_num = ProjectionNum();
 785 |     for (size_t i = 0; i < proj_num; i++)
 786 |     {
 787 |         size_t pose_index = projection_block_.PoseIndex(i);
 788 |         size_t cluster_index = GetPoseCluster(pose_index);
 789 |         size_t point_index = projection_block_.PointIndex(i);
 790 |         size_t local_point_cluster = GetPointLocalCluster(point_index, cluster_index);
 791 | 
 792 |         Mat26 pose_jacobian;
 793 |         Mat23 point_jacobian;
 794 |         Vec6 delta_pose;
 795 |         Vec3 delta_point;
 796 |         Vec2 residual;
 797 |         GetPoseJacobian(i, pose_jacobian);
 798 |         GetPointJacobian(i, point_jacobian);
 799 |         pose_block_.GetDeltaPose(pose_index, delta_pose);
 800 |         GetDeltaPoint(point_index, local_point_cluster, delta_point);
 801 |         GetResidual(i, residual);
 802 |         Vec2 r = pose_jacobian * delta_pose + point_jacobian * delta_point + residual;
 803 |         R += r.squaredNorm();
 804 |     }
 805 | 
 806 |     size_t pose_num = PoseNum();
 807 |     assert(aug_pose_diagonal.rows() == pose_num * 6);
 808 |     for (size_t i = 0; i < pose_num; i++)
 809 |     {
 810 |         Vec6 delta_pose, diagonal;
 811 |         pose_block_.GetDeltaPose(i, delta_pose);
 812 |         diagonal = aug_pose_diagonal.segment(6 * i, 6);
 813 |         R += delta_pose.transpose() * diagonal.cwiseProduct(delta_pose);
 814 |     }
 815 | 
 816 |     size_t point_num = PointNum();
 817 |     for (size_t i = 0; i < point_num; i++)
 818 |     {
 819 |         std::vector<Vec3> const &diagonals = aug_point_diagonal[i];
 820 |         for (size_t j = 0; j < diagonals.size(); j++)
 821 |         {
 822 |             Vec3 aug_diagonal = diagonals[j];
 823 |             Vec3 delta_point;
 824 |             GetDeltaPoint(i, j, delta_point);
 825 |             R += delta_point.transpose() * aug_diagonal.cwiseProduct(delta_point);
 826 |         }
 827 |     }
 828 | 
 829 |     return R;
 830 | }
 831 | 
 832 | double StochasticBAProblem::EvaluateRSquare2(VecX const &aug_diagonal)
 833 | {
 834 |     double R = 0;
 835 |     size_t proj_num = ProjectionNum();
 836 |     double max_error = 0;
 837 |     for (size_t i = 0; i < proj_num; i++)
 838 |     {
 839 |         size_t pose_index = projection_block_.PoseIndex(i);
 840 |         size_t point_index = projection_block_.PointIndex(i);
 841 |         Mat26 pose_jacobian;
 842 |         Mat23 point_jacobian;
 843 |         Vec6 delta_pose;
 844 |         Vec3 delta_point;
 845 |         Vec2 residual;
 846 |         GetPoseJacobian(i, pose_jacobian);
 847 |         GetPointJacobian(i, point_jacobian);
 848 |         pose_block_.GetDeltaPose(pose_index, delta_pose);
 849 |         point_block_.GetDeltaPoint(point_index, delta_point);
 850 |         GetResidual(i, residual);
 851 |         Vec2 r = pose_jacobian * delta_pose + point_jacobian * delta_point + residual;
 852 |         std::vector<size_t> clusters;
 853 |         GetPointClusters(point_index, clusters);
 854 |         double error = r.squaredNorm();
 855 |         max_error = std::max(max_error, error);
 856 |         if (error > 1e3)
 857 |         {
 858 |             continue;
 859 |             std::cout << "[EvaluateRSquare2] " << pose_index << ", " << point_index << ", "
 860 |                       << r.squaredNorm() << ", " << clusters.size() << "\n";
 861 |         }
 862 |         R += r.squaredNorm();
 863 |     }
 864 |     //    std::cout << "[EvaluateRSquare2] max_error = " << max_error << "\n";
 865 | 
 866 |     size_t pose_num = PoseNum();
 867 |     size_t point_num = PointNum();
 868 |     assert(aug_diagonal.rows() == pose_num * 6 + point_num * 3);
 869 |     for (size_t i = 0; i < pose_num; i++)
 870 |     {
 871 |         Vec6 delta_pose, diagonal;
 872 |         pose_block_.GetDeltaPose(i, delta_pose);
 873 |         diagonal = aug_diagonal.segment(6 * i, 6);
 874 |         R += delta_pose.transpose() * diagonal.cwiseProduct(delta_pose);
 875 |     }
 876 |     for (size_t i = 0; i < point_num; i++)
 877 |     {
 878 |         Vec3 delta_point, diagonal;
 879 |         point_block_.GetDeltaPoint(i, delta_point);
 880 |         diagonal = aug_diagonal.segment(6 * pose_num + 3 * i, 3);
 881 |         R += delta_point.transpose() * diagonal.cwiseProduct(delta_point);
 882 |     }
 883 | 
 884 |     return R;
 885 | }
 886 | 
 887 | bool StochasticBAProblem::StepAccept()
 888 | {
 889 |     return last_square_error_ > square_error_;
 890 | }
 891 | 
 892 | bool StochasticBAProblem::Initialize(BundleBlock const &bundle_block)
 893 | {
 894 |     if (!BAProblem::Initialize(bundle_block))
 895 |         return false;
 896 |     InitializeCluster();
 897 |     return true;
 898 | }
 899 | 
 900 | void StochasticBAProblem::InitializeCluster()
 901 | {
 902 |     std::vector<size_t> nodes(PoseNum());
 903 |     std::unordered_map<size_t, std::unordered_map<size_t, double>> edges;
 904 |     std::iota(nodes.begin(), nodes.end(), 0);
 905 | 
 906 |     std::unordered_map<size_t, std::unordered_map<size_t, std::vector<size_t>>>::const_iterator it1 = common_point_map_.begin();
 907 |     for (; it1 != common_point_map_.end(); it1++)
 908 |     {
 909 |         size_t pose_index1 = it1->first;
 910 |         size_t point_num1 = pose_projection_map_.find(pose_index1)->second.size();
 911 |         std::unordered_map<size_t, double> edge_map;
 912 |         std::unordered_map<size_t, std::vector<size_t>> const &map = it1->second;
 913 |         std::unordered_map<size_t, std::vector<size_t>>::const_iterator it2 = map.begin();
 914 |         for (; it2 != map.end(); it2++)
 915 |         {
 916 |             size_t pose_index2 = it2->first;
 917 |             if (pose_index1 == pose_index2)
 918 |                 continue;
 919 |             std::vector<size_t> const &points = it2->second;
 920 |             size_t point_num2 = pose_projection_map_.find(pose_index2)->second.size();
 921 |             edge_map[pose_index2] = double(points.size()) / (point_num1 + point_num2 - points.size());
 922 |         }
 923 |         edges[pose_index1] = edge_map;
 924 |     }
 925 |     cluster_->Initialize(nodes, edges);
 926 | }
 927 | 
 928 | void StochasticBAProblem::RunCluster()
 929 | {
 930 |     std::vector<std::pair<size_t, size_t>> initial_pairs;
 931 |     cluster_->GetEdgesAcrossClusters(initial_pairs);
 932 |     cluster_->Reinitialize();
 933 |     if (complementary_clustering_)
 934 |         cluster_->StochasticCluster(initial_pairs);
 935 |     else
 936 |         cluster_->StochasticCluster();
 937 |     double broken_edge_weight = 0.0;
 938 |     for (size_t i = 0; i < initial_pairs.size(); i++)
 939 |     {
 940 |         size_t index1 = initial_pairs[i].first;
 941 |         size_t index2 = initial_pairs[i].second;
 942 |         broken_edge_weight += cluster_->EdgeWeight(index1, index2);
 943 |     }
 944 |     //    connectivity_sample_ratio_ = 1.0 - broken_edge_weight / cluster_->SumEdgeWeight();
 945 |     connectivity_sample_ratio_ = 1.0 - initial_pairs.size() / double(cluster_->EdgeNum());
 946 | 
 947 |     cluster_->GetClusters(cluster_poses_);
 948 |     size_t cluster_num = cluster_poses_.size();
 949 |     pose_cluster_map_.resize(PoseNum(), 0);
 950 |     for (size_t i = 0; i < cluster_num; i++)
 951 |     {
 952 |         std::vector<size_t> const &pose_cluster = cluster_poses_[i];
 953 |         for (size_t j = 0; j < pose_cluster.size(); j++)
 954 |         {
 955 |             size_t pose_index = pose_cluster[j];
 956 |             assert(pose_index < PoseNum() && "[RunCluster] Pose index out of range");
 957 |             pose_cluster_map_[pose_index] = i;
 958 |         }
 959 |     }
 960 | 
 961 |     ClearPointMeta();
 962 |     size_t point_num = PointNum();
 963 |     cluster_points_.clear();
 964 |     point_cluster_map_.clear();
 965 |     cluster_points_.resize(cluster_num, std::vector<size_t>());
 966 |     point_cluster_map_.resize(point_num, std::vector<size_t>());
 967 |     point_meta_.resize(point_num);
 968 |     for (size_t i = 0; i < point_num; i++)
 969 |     {
 970 |         std::vector<size_t> clusters;
 971 |         GetPointClusters(i, clusters);
 972 |         point_cluster_map_[i] = clusters;
 973 |         point_meta_[i] = new PointMeta(clusters.size());
 974 |         for (size_t j = 0; j < clusters.size(); j++)
 975 |         {
 976 |             size_t cluster_index = clusters[j];
 977 |             cluster_points_[cluster_index].push_back(i);
 978 |         }
 979 |     }
 980 | }
 981 | 
 982 | void StochasticBAProblem::Print()
 983 | {
 984 |     double delta_loss = last_square_error_ - square_error_;
 985 |     double max_gradient = MaxGradient();
 986 |     double step = Step();
 987 |     double modualarity = cluster_->Modularity();
 988 |     std::vector<std::vector<size_t>> clusters;
 989 |     cluster_->GetClusters(clusters);
 990 |     double mean_error, median_error, max_error;
 991 |     ReprojectionError(mean_error, median_error, max_error, true);
 992 |     std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
 993 |     std::chrono::duration<double> elapse = now - time_;
 994 |     double duration = elapse.count();
 995 | 
 996 |     size_t width = 9;
 997 |     std::string status = step_accept_ ? std::string("[Update] ") : std::string("[Reject] ");
 998 |     std::stringstream local_stream;
 999 |     local_stream << std::setprecision(3) << std::scientific
1000 |                  << status << std::left << std::setw(3) << iter_ << ", "
1001 |                  << "d: " << std::setw(width + 1) << delta_loss << ", "
1002 |                  << "F0: " << std::setw(width) << last_square_error_ << ", "
1003 |                  << "F1: " << std::setw(width) << square_error_ << ", "
1004 |                  << "g: " << std::setw(width) << max_gradient << ", "
1005 |                  << "mu: " << std::setw(width) << mu_ << ", "
1006 |                  << "h: " << std::setw(width) << step << ", "
1007 |                  << std::setprecision(3) << std::fixed
1008 |                  << "me: " << std::setw(6) << median_error << ", "
1009 |                  << "ae: " << std::setw(6) << mean_error << ", "
1010 |                  << "B: " << std::setw(2) << batch_size_ << ", "
1011 |                  << "In: " << std::setw(2) << inner_step_ << ", "
1012 |                  << "#C: " << std::setw(4) << clusters.size() << ", "
1013 |                  << "Q: " << std::setw(5) << modualarity << ", "
1014 |                  << "s: " << std::setw(5) << connectivity_sample_ratio_ << ", "
1015 |                  << std::setprecision(1) << std::fixed
1016 |                  << "t: " << std::setw(5) << duration << "\n";
1017 |     std::cout << local_stream.str();
1018 |     stream_ << local_stream.str();
1019 | }
1020 | 
1021 | void StochasticBAProblem::SaveCameraCluster(std::string const &save_path)
1022 | {
1023 |     size_t camera_num = PoseNum();
1024 |     std::vector<std::vector<size_t>> clusters;
1025 |     cluster_->GetClusters(clusters);
1026 |     size_t cluster_num = clusters.size();
1027 | 
1028 |     std::ofstream fout(save_path);
1029 |     fout << camera_num << "\t" << cluster_num << "\n";
1030 | 
1031 |     for (size_t i = 0; i < cluster_num; i++)
1032 |     {
1033 |         size_t cluster_index = i;
1034 |         std::vector<size_t> const &camera_indexes = clusters[i];
1035 |         for (size_t j = 0; j < camera_indexes.size(); j++)
1036 |         {
1037 |             size_t camera_index = camera_indexes[j];
1038 |             size_t group_index = GetPoseGroup(camera_index);
1039 |             Vec6 intrinsic;
1040 |             GetIntrinsic(group_index, intrinsic);
1041 |             double focal = intrinsic(0);
1042 |             double u = intrinsic(1);
1043 |             double v = intrinsic(2);
1044 |             fout << camera_index << "\t" << cluster_index << "\t" << focal << "\t" << u << "\t" << v << "\n";
1045 |             Vec3 angle_axis, translation;
1046 |             pose_block_.GetPose(camera_index, angle_axis, translation);
1047 |             Mat3 rotation = AngleAxis2Matrix(angle_axis);
1048 |             Vec3 center = -rotation.transpose() * translation;
1049 |             fout << rotation << "\n"
1050 |                  << center(0) << " " << center(1) << " " << center(2) << "\n";
1051 |         }
1052 |     }
1053 |     fout.close();
1054 | }
1055 | 
1056 | void StochasticBAProblem::SteepestDescentCorrection(size_t const point_index)
1057 | {
1058 |     std::vector<size_t> const &cluster_indexes = point_cluster_map_[point_index];
1059 |     size_t cluster_num = cluster_indexes.size();
1060 |     assert(cluster_num > 0 && "[EvaluateDeltaPoint] Zero cluster number");
1061 | 
1062 |     if (cluster_num == 1)
1063 |         return;
1064 | 
1065 |     MatX H = MatX::Zero(3 * (cluster_num - 1), 3 * cluster_num);
1066 |     for (size_t i = 0; i < cluster_num - 1; i++)
1067 |     {
1068 |         for (size_t j = 0; j < 3; j++)
1069 |         {
1070 |             H(i * 3 + j, j) = 1;
1071 |             H(i * 3 + j, (i + 1) * 3 + j) = -1;
1072 |         }
1073 |     }
1074 | 
1075 |     VecX C_inv_g = VecX::Zero(3 * cluster_num);
1076 |     MatX C_inv = MatX::Identity(3 * cluster_num, 3 * cluster_num);
1077 |     for (size_t i = 0; i < cluster_num; i++)
1078 |     {
1079 |         Mat3 JpJp;
1080 |         GetJpJp(point_index, i, JpJp);
1081 | 
1082 |         Vec3 Jpe;
1083 |         GetJpe(point_index, i, Jpe);
1084 | 
1085 |         for (size_t j = 0; j < 3; j++)
1086 |         {
1087 |             if (JpJp(j, j) < EPSILON)
1088 |                 return;
1089 |             C_inv(3 * i + j, 3 * i + j) = 1.0 / JpJp(j, j);
1090 |             C_inv_g(3 * i + j) = Jpe(j) / JpJp(j, j);
1091 |         }
1092 |     }
1093 | 
1094 |     MatX HCH = H * C_inv * H.transpose();
1095 |     MatX HCH_inv = HCH.inverse();
1096 |     VecX g_correct = -H.transpose() * HCH_inv * H * C_inv_g;
1097 |     if (!IsNumericalValid(g_correct))
1098 |     {
1099 |         std::cout << "[StochasticBAProblem::GradientCorrection] Numerical invalid.\n";
1100 |         return;
1101 |     }
1102 |     for (size_t i = 0; i < cluster_num; i++)
1103 |     {
1104 |         Vec3 Jpe;
1105 |         GetJpe(point_index, i, Jpe);
1106 |         SetJpe(point_index, i, Jpe + g_correct.segment(3 * i, 3));
1107 |     }
1108 | }
1109 | 
1110 | void StochasticBAProblem::SteepestDescentCorrection()
1111 | {
1112 |     size_t point_num = PointNum();
1113 | 
1114 | #pragma omp parallel for
1115 |     for (size_t i = 0; i < point_num; i++)
1116 |     {
1117 |         SteepestDescentCorrection(i);
1118 |     }
1119 | }
1120 | 


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