Skip to content

Commit

Permalink
add license identifiers
Browse files Browse the repository at this point in the history
  • Loading branch information
koide3 committed Jun 27, 2024
1 parent dbcfbc1 commit eb0ed80
Show file tree
Hide file tree
Showing 24 changed files with 113 additions and 20 deletions.
21 changes: 21 additions & 0 deletions .github/workflows/license.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
name: license

on:
push:
branches: [ master ]
paths-ignore: '**.md'
pull_request:
branches: [ master ]
paths-ignore: '**.md'

jobs:
license_check:
name: License check
runs-on: ubuntu-latest

steps:
- uses: actions/checkout@v4

- name: Check license
run: |
find include/gtsam_points src/gtsam_points -type f | xargs -I filename bash -c 'if ! grep -q Copyright filename; then echo filename : lisence not found; exit 1; fi'
2 changes: 2 additions & 0 deletions include/gtsam_points/ann/knn_result.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once

#include <iostream>
Expand Down
3 changes: 3 additions & 0 deletions include/gtsam_points/cuda/cuda_malloc_async.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#if (CUDA_VERSION < 11000)
Expand Down
3 changes: 3 additions & 0 deletions include/gtsam_points/cuda/gl_buffer_map.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#include <cstdlib>
Expand Down
3 changes: 3 additions & 0 deletions include/gtsam_points/cuda/kernels/inlier_access_kernel.cuh
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#include <iostream>
Expand Down
5 changes: 4 additions & 1 deletion include/gtsam_points/optimizers/dogleg_optimizer_ext.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,10 @@ class GTSAM_EXPORT DoglegOptimizerExt : public gtsam::NonlinearOptimizer {
* @param initialValues The initial variable assignments
* @param params The optimization parameters
*/
DoglegOptimizerExt(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params = gtsam::DoglegParams());
DoglegOptimizerExt(
const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& initialValues,
const gtsam::DoglegParams& params = gtsam::DoglegParams());

/** Standard constructor, requires a nonlinear factor graph, initial
* variable assignments, and optimization parameters. For convenience this
Expand Down
13 changes: 10 additions & 3 deletions include/gtsam_points/optimizers/dogleg_optimizer_ext_impl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,8 @@ struct GTSAM_EXPORT DoglegOptimizerImplExt {
* @param dx_n The Gauss-Newton point
* @return The dogleg point \f$ \delta x_d \f$
*/
static gtsam::VectorValues ComputeDoglegPoint(double delta, const gtsam::VectorValues& dx_u, const gtsam::VectorValues& dx_n, const bool verbose = false);
static gtsam::VectorValues
ComputeDoglegPoint(double delta, const gtsam::VectorValues& dx_u, const gtsam::VectorValues& dx_n, const bool verbose = false);

/** Compute the point on the line between the steepest descent point and the
* Newton's method point intersecting the trust region boundary.
Expand Down Expand Up @@ -158,7 +159,11 @@ typename DoglegOptimizerImplExt::IterationResult DoglegOptimizerImplExt::Iterate
IterationResult result;

bool stay = true;
enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration
enum {
NONE,
INCREASED_DELTA,
DECREASED_DELTA
} lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration
while (stay) {
gttic(Dog_leg_point);
// Compute dog leg point
Expand Down Expand Up @@ -189,7 +194,9 @@ typename DoglegOptimizerImplExt::IterationResult DoglegOptimizerImplExt::Iterate
gttic(adjust_delta);
// Compute gain ratio. Here we take advantage of the invariant that the
// Bayes' net error at zero is equal to the nonlinear error
const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ? 0.5 : (f_error - result.f_error) / (M_error - new_M_error);
const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15
? 0.5
: (f_error - result.f_error) / (M_error - new_M_error);

if (verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl;

Expand Down
3 changes: 3 additions & 0 deletions include/gtsam_points/optimizers/isam2_ext_dummy.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#include <gtsam_points/optimizers/isam2_ext.hpp>
Expand Down
3 changes: 3 additions & 0 deletions include/gtsam_points/types/dummy_frame.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

namespace gtsam_points {
Expand Down
8 changes: 6 additions & 2 deletions include/gtsam_points/util/easy_profiler.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#include <chrono>
Expand Down Expand Up @@ -63,7 +66,8 @@ class EasyProfiler {
times.push_back(std::chrono::high_resolution_clock::now());

if (debug) {
ost << ">> " << label << " (" << std::chrono::duration_cast<std::chrono::nanoseconds>(times.back() - times.front()).count() / 1e6 << "[msec])" << std::endl;
ost << ">> " << label << " (" << std::chrono::duration_cast<std::chrono::nanoseconds>(times.back() - times.front()).count() / 1e6 << "[msec])"
<< std::endl;
}
}

Expand All @@ -78,4 +82,4 @@ class EasyProfiler {
std::ostream& ost;
std::ofstream ofs;
};
} // namespace glim
} // namespace gtsam_points
3 changes: 3 additions & 0 deletions include/gtsam_points/util/easy_profiler_cuda.hpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#pragma once

#include <vector>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/cuda/cuda_stream.cu
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/cuda/cuda_stream.hpp>

#include <iostream>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/cuda/gl_buffer_map.cu
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/cuda/gl_buffer_map.hpp>

#include <cuda_runtime.h>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/types/point_cloud.hpp>
// #include <gtsam_points/factors/intensity_gradients_ivox.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_colored_gicp_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/types/point_cloud.hpp>
// #include <gtsam_points/factors/intensity_gradients_ivox.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_ct_gicp_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/ann/ivox_covariance_estimation.hpp>
#include <gtsam_points/types/point_cloud.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_ct_icp_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/ann/ivox_covariance_estimation.hpp>
#include <gtsam_points/types/point_cloud.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_gicp_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/ann/ivox_covariance_estimation.hpp>
#include <gtsam_points/types/point_cloud.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_loam_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/ann/ivox.hpp>
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/factors/integrated_loam_factor.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/factors/integrated_vgicp_factor.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/factors/integrated_vgicp_factor.hpp>
#include <gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/optimizers/fast_scatter.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/optimizers/fast_scatter.hpp>

#include <unordered_set>
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/optimizers/gaussian_factor_graph_solver.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/optimizers/gaussian_factor_graph_solver.hpp>

#include <gtsam_points/optimizers/linear_solver.hpp>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp>

#include <chrono>
Expand Down Expand Up @@ -71,7 +74,7 @@ const gtsam::Value& IncrementalFixedLagSmootherExtWithFallback::calculateEstimat
try {
const auto& value = smoother->calculateEstimate(key);
auto found = values.find(key);
if(found != values.end()) {
if (found != values.end()) {
found->value = value;
}

Expand Down Expand Up @@ -125,7 +128,7 @@ void IncrementalFixedLagSmootherExtWithFallback::update_fallback_state() {
gtsam::NonlinearFactorGraph next_factors;
next_factors.reserve(factors.size());
for (const auto& factor : factors) {
if(!factors_to_eliminate.count(factor.get())) {
if (!factors_to_eliminate.count(factor.get())) {
next_factors.add(factor);
}
}
Expand All @@ -149,7 +152,7 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
std::unordered_map<char, size_t> max_ids;
for (const auto& value : values) {
const auto found = stamps.find(value.key);
if(found == stamps.end()) {
if (found == stamps.end()) {
std::cerr << "warning: corresponding stamp is not found for " << gtsam::Symbol(value.key) << std::endl;
continue;
}
Expand All @@ -158,14 +161,14 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
const gtsam::Symbol symbol(value.key);

auto min_stamp = min_stamps.find(symbol.chr());
if(min_stamp == min_stamps.end()) {
if (min_stamp == min_stamps.end()) {
min_stamps.emplace_hint(min_stamp, symbol.chr(), stamp);
} else {
min_stamp->second = std::min(min_stamp->second, stamp);
}

auto max_id = max_ids.find(symbol.chr());
if(max_id == max_ids.end()) {
if (max_id == max_ids.end()) {
max_ids.emplace_hint(max_id, symbol.chr(), symbol.index());
} else {
max_id->second = std::max<size_t>(max_id->second, max_id->second);
Expand All @@ -183,7 +186,7 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
// connectivity check
std::unordered_map<gtsam::Key, std::vector<gtsam::NonlinearFactor*>> factormap;
for (const auto& factor : factors) {
for(const auto key: factor->keys()) {
for (const auto key : factor->keys()) {
factormap[key].push_back(factor.get());
}
}
Expand All @@ -204,27 +207,27 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
traverse_queue.pop_back();

for (const auto key : factor->keys()) {
if(traversed_keys.count(key)) {
if (traversed_keys.count(key)) {
continue;
}

traversed_keys.insert(key);
const auto found = factormap.find(key);
if(found != factormap.end()) {
if (found != factormap.end()) {
traverse_queue.insert(traverse_queue.end(), found->second.begin(), found->second.end());
}
}
}

std::unordered_set<gtsam::Key> keys_to_remove;
for(const auto& value: values) {
if(!traversed_keys.count(value.key)) {
for (const auto& value : values) {
if (!traversed_keys.count(value.key)) {
std::cerr << "unreached key found:" << gtsam::Symbol(value.key) << std::endl;
keys_to_remove.insert(value.key);
}
}

for(const auto& key: keys_to_remove) {
for (const auto& key : keys_to_remove) {
values.erase(key);
stamps.erase(key);
}
Expand Down Expand Up @@ -266,7 +269,6 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
std::cout << "factors:" << factors.size() << " fixed:" << new_factors.size() - factors.size() << " values:" << values.size()
<< " stamps:" << stamps.size() << std::endl;


for (const auto& factor : new_factors) {
for (const auto key : factor->keys()) {
if (!values.exists(key)) {
Expand All @@ -278,8 +280,8 @@ void IncrementalFixedLagSmootherExtWithFallback::fallback_smoother() const {
}
}

for(const auto& stamp: stamps) {
if(!values.exists(stamp.first)) {
for (const auto& stamp : stamps) {
if (!values.exists(stamp.first)) {
std::cout << "illegal stamp found!! " << gtsam::Symbol(stamp.first) << std::endl;
}
}
Expand Down
3 changes: 3 additions & 0 deletions src/gtsam_points/util/easy_profiler_cuda.cu
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp)

#include <gtsam_points/util/easy_profiler_cuda.hpp>

#include <cuda_runtime.h>
Expand Down

0 comments on commit eb0ed80

Please sign in to comment.