Main Page
Namespaces
Classes
Files
File List
File Members
cartographer
mapping_2d
scan_matching
mapping_2d/scan_matching/translation_delta_cost_functor.h
Go to the documentation of this file.
1
/*
2
* Copyright 2016 The Cartographer Authors
3
*
4
* Licensed under the Apache License, Version 2.0 (the "License");
5
* you may not use this file except in compliance with the License.
6
* You may obtain a copy of the License at
7
*
8
* http://www.apache.org/licenses/LICENSE-2.0
9
*
10
* Unless required by applicable law or agreed to in writing, software
11
* distributed under the License is distributed on an "AS IS" BASIS,
12
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13
* See the License for the specific language governing permissions and
14
* limitations under the License.
15
*/
16
17
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
18
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
19
20
#include "Eigen/Core"
21
#include "
cartographer/transform/rigid_transform.h
"
22
23
namespace
cartographer
{
24
namespace
mapping_2d {
25
namespace
scan_matching {
26
27
// Computes the cost of translating the initial pose estimate. Cost increases
28
// with the solution's distance from the initial estimate.
29
class
TranslationDeltaCostFunctor
{
30
public
:
31
// Constructs a new TranslationDeltaCostFunctor from the given
32
// 'initial_pose_estimate' (x, y, theta).
33
explicit
TranslationDeltaCostFunctor
(
34
const
double
scaling_factor,
35
const
transform::Rigid2d
& initial_pose_estimate)
36
:
scaling_factor_
(scaling_factor),
37
x_
(initial_pose_estimate.translation().x()),
38
y_
(initial_pose_estimate.translation().y()) {}
39
40
TranslationDeltaCostFunctor
(
const
TranslationDeltaCostFunctor
&) =
delete
;
41
TranslationDeltaCostFunctor
&
operator=
(
const
TranslationDeltaCostFunctor
&) =
42
delete
;
43
44
template
<
typename
T>
45
bool
operator()
(
const
T*
const
pose
, T* residual)
const
{
46
residual[0] =
scaling_factor_
* (pose[0] -
x_
);
47
residual[1] =
scaling_factor_
* (pose[1] -
y_
);
48
return
true
;
49
}
50
51
private
:
52
const
double
scaling_factor_
;
53
const
double
x_
;
54
const
double
y_
;
55
};
56
57
}
// namespace scan_matching
58
}
// namespace mapping_2d
59
}
// namespace cartographer
60
61
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::operator()
bool operator()(const T *const pose, T *residual) const
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:45
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::x_
const double x_
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:53
pose
transform::Rigid3d pose
Definition:
kalman_local_trajectory_builder_test.cc:42
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::y_
const double y_
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:54
cartographer::transform::Rigid2
Definition:
rigid_transform.h:33
cartographer
Definition:
blocking_queue.h:29
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::TranslationDeltaCostFunctor
TranslationDeltaCostFunctor(const double scaling_factor, const transform::Rigid2d &initial_pose_estimate)
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:33
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:29
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::operator=
TranslationDeltaCostFunctor & operator=(const TranslationDeltaCostFunctor &)=delete
cartographer::mapping_2d::scan_matching::TranslationDeltaCostFunctor::scaling_factor_
const double scaling_factor_
Definition:
mapping_2d/scan_matching/translation_delta_cost_functor.h:52
rigid_transform.h
cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:59