forked from Kitware/VeloView
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'feature/TemporalTransformsWriter' into 'kitware-master'
Writer for vtkTemporalTransforms See merge request bjacquet/VeloView-kwinternal!267
- Loading branch information
Showing
10 changed files
with
361 additions
and
33 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| #include "vtkTemporalTransforms.h" | ||
| #include "vtkTemporalTransformsWriter.h" | ||
|
|
||
| #include "vtkTransform.h" | ||
| #include "vtkNew.h" | ||
| #include <vtkObjectFactory.h> | ||
| #include "vtkInformationVector.h" | ||
| #include "vtkInformation.h" | ||
| #include <iostream> | ||
|
|
||
| #include "vtkConversions.h" | ||
|
|
||
| //----------------------------------------------------------------------------- | ||
| vtkStandardNewMacro(vtkTemporalTransformsWriter) | ||
|
|
||
| //----------------------------------------------------------------------------- | ||
| vtkTemporalTransformsWriter::~vtkTemporalTransformsWriter() | ||
| { | ||
| if (this->FileName) | ||
| { | ||
| delete[] this->FileName; | ||
| } | ||
| } | ||
|
|
||
| //----------------------------------------------------------------------------- | ||
| int vtkTemporalTransformsWriter::RequestData(vtkInformation *vtkNotUsed(request), | ||
| vtkInformationVector **inputVector, | ||
| vtkInformationVector *vtkNotUsed(outputVector)) | ||
| { | ||
| vtkInformation *inInfo = inputVector[0]->GetInformationObject(0); | ||
| vtkPolyData *polyData = vtkPolyData::SafeDownCast( | ||
| inInfo->Get(vtkDataObject::DATA_OBJECT())); | ||
|
|
||
| if (polyData == nullptr) | ||
| { | ||
| vtkErrorMacro("Input is not a vtkPolyData"); | ||
| return 0; | ||
| } | ||
|
|
||
| auto transforms = vtkTemporalTransforms::CreateFromPolyData(polyData); | ||
| if (transforms == nullptr) | ||
| { | ||
| vtkErrorMacro("Input is not a vtkTemporalTransforms"); | ||
| return 0; | ||
| } | ||
|
|
||
| vtkDataArray* time = transforms->GetTimeArray(); | ||
|
|
||
| std::ofstream file(this->FileName); | ||
| // TODO: when we support reading a CSV with commented line, add such comment: | ||
| // # Pose trajectory format, time in s, angles in degree, position in meters | ||
| // # Recompose the rotation part of the pose using: | ||
| // # R = Rz(yaw) * Ry(pitch) * Rx(roll) | ||
| // # a point exprimed in the Lidar reference frame can be exprimed in a fixed | ||
| // # reference frame using: X_fixed = R(t) * X_lidar + [x(t), y(t), z(t)]^T | ||
| file << "Time,Rx(Roll),Ry(Pitch),Rz(Yaw),X,Y,Z" << endl; | ||
|
|
||
| for (unsigned int i = 0; i < transforms->GetNumberOfPoints(); i++) | ||
| { | ||
| double t = time->GetTuple1(i); | ||
| auto transform = transforms->GetTransform(i); | ||
| std::pair<Eigen::Vector3d, Eigen::Vector3d> towrite = GetPoseParamsFromTransform(transform); | ||
| // We *need* to take care than the user might want to export a trajectory | ||
| // that was produced by projecting GPS coordinate into UTM, thus giving huge | ||
| // coordinates (thousands of kilometers). | ||
| // So make sure that you always show up to N decimals after the point. | ||
| // (not just N significant digits because many digits could be before the | ||
| // point). | ||
| // Possible optimization: hide useless trailing zeros which take up space. | ||
| // (at the cost of breaking the columns alignement that helps a human parse) | ||
| file << std::fixed << std::setprecision(17) | ||
| << t | ||
| << "," << towrite.first[0] | ||
| << "," << towrite.first[1] | ||
| << "," << towrite.first[2] | ||
| << "," << towrite.second[0] | ||
| << "," << towrite.second[1] | ||
| << "," << towrite.second[2] | ||
| << std::endl; | ||
| } | ||
|
|
||
| file.close(); | ||
| return 1; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,44 @@ | ||
| // Copyright 2019 Kitware SAS. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef VTKTEMPORALTRANSFORMSWRITER_H | ||
| #define VTKTEMPORALTRANSFORMSWRITER_H | ||
|
|
||
| // #include <vtkPolyDataAlgorithm.h> | ||
| #include <vtkPolyDataWriter.h> | ||
|
|
||
| // Inspired by vtkObjWriter | ||
| class VTK_EXPORT vtkTemporalTransformsWriter : public vtkPolyDataWriter | ||
| { | ||
| public: | ||
| static vtkTemporalTransformsWriter* New(); | ||
| vtkTypeMacro(vtkTemporalTransformsWriter, vtkPolyDataWriter) | ||
|
|
||
| vtkSetStringMacro(FileName) | ||
| vtkGetStringMacro(FileName) | ||
|
|
||
| int RequestData(vtkInformation *, vtkInformationVector **, vtkInformationVector *); | ||
|
|
||
| protected: | ||
| vtkTemporalTransformsWriter() = default; | ||
| ~vtkTemporalTransformsWriter(); | ||
|
|
||
| private: | ||
| vtkTemporalTransformsWriter(const vtkTemporalTransformsWriter&) = delete; | ||
| void operator =(const vtkTemporalTransformsWriter&) = delete; | ||
|
|
||
| char* FileName = nullptr; | ||
| }; | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
128 changes: 128 additions & 0 deletions
128
VelodyneHDL/Testing/TestTemporalTransformsReaderWriter.cxx
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,128 @@ | ||
| #include "vtkTemporalTransformsReader.h" | ||
| #include "vtkTemporalTransformsWriter.h" | ||
|
|
||
| #include <iostream> | ||
| #include <iomanip> | ||
| #include <stdio.h> | ||
|
|
||
| #include "vtkPolyData.h" | ||
|
|
||
| #include "TestHelpers.h" | ||
|
|
||
| const double epsilon = 1e-6; | ||
|
|
||
| bool file_exists(const char *path) | ||
| { | ||
| std::ifstream infile(path); | ||
| return infile.good(); | ||
| } | ||
|
|
||
| bool check_mm04_orbslam2_no_loop_closure(char* path) | ||
| { | ||
| auto reader = vtkSmartPointer<vtkTemporalTransformsReader>::New(); | ||
| reader->SetFileName(path); | ||
| reader->Update(); | ||
| vtkPolyData* read = reader->GetOutput(); | ||
|
|
||
| bool allgood = true; | ||
| allgood &= read->GetNumberOfCells() == 1; | ||
| allgood &= read->GetNumberOfPoints() == 242; | ||
| if (!allgood) { | ||
| // no need to go further (else we can now ask for point 241) | ||
| return 0; | ||
| } | ||
|
|
||
| vtkDoubleArray* points = vtkDoubleArray::SafeDownCast(read->GetPoints()->GetData()); | ||
| allgood &= points != nullptr; | ||
| double* lastPointXYZ = points->GetTuple3(241); | ||
| const double referenceLastPointXYZ[3] = { 0.1678862, -0.0361966, -0.0566818 }; | ||
| allgood &= compare(lastPointXYZ, referenceLastPointXYZ, 3, epsilon); | ||
|
|
||
| vtkDoubleArray* time = vtkDoubleArray::SafeDownCast(read->GetPointData()->GetArray("Time")); | ||
| allgood &= time != nullptr; | ||
| double lastTime = time->GetTuple1(241); | ||
| const double referenceTime = 136.457; | ||
| allgood &= compare(&lastTime, &referenceTime, 1, epsilon); | ||
|
|
||
| vtkDoubleArray* orientation = vtkDoubleArray::SafeDownCast(read->GetPointData()->GetArray("Orientation(AxisAngle)")); | ||
| allgood &= orientation != nullptr; | ||
| double* lastOrientation = orientation->GetTuple4(241); | ||
| const double referenceLastOrientation[4] = { 0.711088177611299, 0.335114389446281, 0.618103510463650, 0.022307328124517 }; | ||
| allgood &= compare(lastOrientation, referenceLastOrientation, 4, epsilon); | ||
|
|
||
| return allgood; | ||
| } | ||
|
|
||
|
|
||
| bool read_write_trajectory(char* to_read, char* to_write) | ||
| { | ||
| auto reader = vtkSmartPointer<vtkTemporalTransformsReader>::New(); | ||
| reader->SetFileName(to_read); | ||
| reader->Update(); | ||
|
|
||
| auto writer = vtkSmartPointer<vtkTemporalTransformsWriter>::New(); | ||
| writer->SetInputConnection(0, reader->GetOutputPort()); | ||
| writer->SetFileName(to_write); | ||
| writer->Update(); | ||
| return 1; | ||
| } | ||
|
|
||
| int main(int argc, char* argv[]) | ||
| { | ||
| if (argc != 3) | ||
| { | ||
| std::cerr << "Wrong number of arguments. Usage: " << argv[0] | ||
| << " <path to mm04/orbslam2-no-loop-closure.csv>" | ||
| << " <path to a writable file that will be destroyed>" | ||
| << std::endl; | ||
| return 1; | ||
| } | ||
|
|
||
| char* referenceTrajectory = argv[1]; | ||
| char* temporaryFile = argv[2]; // ! will be deleted | ||
|
|
||
| // First, check that the reader works: | ||
| if (! check_mm04_orbslam2_no_loop_closure(referenceTrajectory)) | ||
| { | ||
| std::cout << "Reading trajectory using vtkTemporalTransformsReader" | ||
| << " does not seem to work" << std::endl; | ||
| return 1; | ||
| } | ||
|
|
||
| // Then prepare to test the writer | ||
| if (file_exists(temporaryFile)) | ||
| { | ||
| const int result = std::remove(temporaryFile); | ||
| if (result != 0) | ||
| { | ||
| std::cout << "Failed to delete existing file: " << temporaryFile | ||
| << " cannot test writer" << std::endl; | ||
| return 1; | ||
| } | ||
| } | ||
|
|
||
| // Then test the writer | ||
| read_write_trajectory(referenceTrajectory, temporaryFile); | ||
| if (!file_exists(temporaryFile)) | ||
| { | ||
| std::cout << "Failed to produce file using writer: " << temporaryFile << std::endl; | ||
| return 1; | ||
| } | ||
|
|
||
| if (! check_mm04_orbslam2_no_loop_closure(temporaryFile)) | ||
| { | ||
| std::cout << "Reading file written using vtkTemporalTransformsWriter" | ||
| " does not seem to work" << std::endl; | ||
| return 1; | ||
| } | ||
|
|
||
| const int result = std::remove(temporaryFile); | ||
| if (result != 0) | ||
| { | ||
| std::cout << "Failed to cleanup by deleting existing file: " | ||
| << temporaryFile << std::endl; | ||
| return 1; | ||
| } | ||
|
|
||
| return 0; | ||
| } |
Oops, something went wrong.