1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
|
#include "manif/SO2.h"
#include "manif/algorithms/average.h"
#include <vector>
#include <iostream>
/**
* @brief An example of the use of the average biinvariant algorithm.
*/
int main(int argc, char**)
{
if (argc > 1)
{
std::cout << "Usage: .so2_average\n";
}
// Generate 4 points around PI/2
std::vector<manif::SO2d> points;
points.emplace_back( MANIF_PI/4.);
points.emplace_back(3.*MANIF_PI/8.);
points.emplace_back(5.*MANIF_PI/8.);
points.emplace_back(3.*MANIF_PI/4.);
std::cout << "Initial points:\n";
for (const auto& p : points)
std::cout << p.angle() << "\n";
auto average = manif::average_biinvariant(points);
std::cout << "Average point:\n";
std::cout << average.angle() << "\n";
return 0;
}
|
