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
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
/***********************************************/
/**
* @file instrumentStarCamera2RollPitchYaw.cpp
*
* @brief Compute roll, pitch, yaw angles.
*
* @author Torsten Mayer-Guerr
* @date 2017-06-14
*
*/
/***********************************************/
// Latex documentation
#define DOCSTRING docstring
static const char *docstring = R"(
Compute roll, pitch, yaw angles from \configFile{inputfileStarCamera}{instrument} data.
Optional the angles are computed relative
to a \configFile{inputfileStarCameraReference}{instrument}.
See also \program{SimulateStarCamera}.
)";
/***********************************************/
#include "programs/program.h"
#include "files/fileInstrument.h"
/***** CLASS ***********************************/
/** @brief Compute roll, pitch, yaw angles.
* @ingroup programsGroup */
class InstrumentStarCamera2RollPitchYaw
{
public:
void run(Config &config, Parallel::CommunicatorPtr comm);
};
GROOPS_REGISTER_PROGRAM(InstrumentStarCamera2RollPitchYaw, PARALLEL, "Compute roll, pitch, yaw angles", Instrument)
/***********************************************/
void InstrumentStarCamera2RollPitchYaw::run(Config &config, Parallel::CommunicatorPtr comm)
{
try
{
FileName fileNameOut;
FileName fileNameStarCamera, fileNameReference;
readConfig(config, "outputfileInstrument", fileNameOut, Config::MUSTSET, "", "roll, pitch, yaw [rad], VECTOR3D");
readConfig(config, "inputfileStarCamera", fileNameStarCamera, Config::MUSTSET, "", "");
readConfig(config, "inputfileStarCameraReference", fileNameReference, Config::OPTIONAL, "", "nominal orientation");
if(isCreateSchema(config)) return;
logStatus<<"read star camera data and compute roll, pitch, yaw"<<Log::endl;
InstrumentFile starCamera1File(fileNameStarCamera);
InstrumentFile starCamera2File(fileNameReference);
InstrumentFile::checkArcCount({starCamera1File, starCamera2File});
std::vector<Arc> arcList(starCamera1File.arcCount());
Parallel::forEach(arcList, [&](UInt arcNo)
{
StarCameraArc starCamera1 = starCamera1File.readArc(arcNo);
StarCameraArc starCamera2 = starCamera2File.readArc(arcNo);
Arc::checkSynchronized({starCamera1, starCamera2});
Vector3dArc arc;
for(UInt i=0; i<starCamera1.size(); i++)
{
if(starCamera2.size())
starCamera1.at(i).rotary = inverse(starCamera2.at(i).rotary) * starCamera1.at(i).rotary;
Angle roll, pitch, yaw;
inverse(starCamera1.at(i).rotary).cardan(roll, pitch, yaw);
Vector3dEpoch epoch;
epoch.time = starCamera1.at(i).time;
epoch.vector3d = Vector3d(roll, pitch, yaw);
arc.push_back(epoch);
}
return arc;
}, comm); // forEach
if(Parallel::isMaster(comm))
{
logStatus<<"write angles to file <"<<fileNameOut<<">"<<Log::endl;
InstrumentFile::write(fileNameOut, arcList);
Arc::printStatistics(arcList);
}
}
catch(std::exception &e)
{
GROOPS_RETHROW(e)
}
}
/***********************************************/
|
