generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix: trajectory path is now relative to the global frame

Browse Source
This commit is contained in:
ozantonkal 2013-07-29 11:01:59 +02:00
parent 3fe5984e46
commit 250dac5b71
1 changed files with 9 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
modules/viz/src/shape_widgets.cpp
View File

@ -1141,9 +1141,6 @@ struct cv::viz::TrajectoryWidget::ApplyPath
{ {
static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3f> &path) static void applyPath(vtkSmartPointer<vtkPolyData> poly_data, vtkSmartPointer<vtkAppendPolyData> append_filter, const std::vector<Affine3f> &path)
{ {
vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
mat_trans->Identity();
vtkIdType nr_points = path.size(); vtkIdType nr_points = path.size();
for (vtkIdType i = 0; i < nr_points; ++i) for (vtkIdType i = 0; i < nr_points; ++i)
@ -1154,7 +1151,8 @@ struct cv::viz::TrajectoryWidget::ApplyPath
// Transform the default coordinate frame // Transform the default coordinate frame
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New(); vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
transform->PreMultiply(); transform->PreMultiply();
vtkMatrix4x4::Multiply4x4(convertToVtkMatrix(path[i].matrix), mat_trans, mat_trans); vtkSmartPointer<vtkMatrix4x4> mat_trans = vtkSmartPointer<vtkMatrix4x4>::New();
mat_trans = convertToVtkMatrix(path[i].matrix);
transform->SetMatrix(mat_trans); transform->SetMatrix(mat_trans);
vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New(); vtkSmartPointer<vtkTransformPolyDataFilter> filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
@ -1185,13 +1183,12 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, i
points->SetNumberOfPoints(nr_points); points->SetNumberOfPoints(nr_points);
polyLine->GetPointIds()->SetNumberOfIds(nr_points); polyLine->GetPointIds()->SetNumberOfIds(nr_points);
Vec3f last_pos(0.0f,0.0f,0.0f);
Vec3f *data_beg = vtkpoints_data<float>(points); Vec3f *data_beg = vtkpoints_data<float>(points);
for (vtkIdType i = 0; i < nr_points; ++i) for (vtkIdType i = 0; i < nr_points; ++i)
{ {
last_pos = path[i] * last_pos; Vec3f cam_pose = path[i].translation();
*data_beg++ = last_pos; *data_beg++ = cam_pose;
polyLine->GetPointIds()->SetId(i,i); polyLine->GetPointIds()->SetId(i,i);
} }
@ -1257,7 +1254,6 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, f
{ {
vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New(); vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
vtkIdType nr_poses = path.size(); vtkIdType nr_poses = path.size();
Point3f last_pos(0.0f,0.0f,0.0f);
// Create color arrays // Create color arrays
vtkSmartPointer<vtkUnsignedCharArray> line_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New(); vtkSmartPointer<vtkUnsignedCharArray> line_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
@ -1290,7 +1286,7 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, f
for (vtkIdType i = 0; i < nr_poses; ++i) for (vtkIdType i = 0; i < nr_poses; ++i)
{ {
Point3f new_pos = path[i] * last_pos; Point3f new_pos = path[i].translation();
vtkSmartPointer<vtkSphereSource> sphere_source = vtkSmartPointer<vtkSphereSource>::New(); vtkSmartPointer<vtkSphereSource> sphere_source = vtkSmartPointer<vtkSphereSource>::New();
sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z); sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z);
@ -1300,7 +1296,6 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, f
sphere_source->Update(); sphere_source->Update();
sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init); sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init);
appendFilter->AddInputConnection(sphere_source->GetOutputPort()); appendFilter->AddInputConnection(sphere_source->GetOutputPort());
last_pos = new_pos;
continue; continue;
} }
else else
@ -1311,7 +1306,9 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, f
appendFilter->AddInputConnection(sphere_source->GetOutputPort()); appendFilter->AddInputConnection(sphere_source->GetOutputPort());
} }
Vec3f v = last_pos - new_pos;
Affine3f relativeAffine = path[i].inv() * path[i-1];
Vec3f v = path[i].rotation() * relativeAffine.translation();
v = normalize(v) * line_length; v = normalize(v) * line_length;
vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::New(); vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::New();
@ -1321,7 +1318,6 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, f
line_source->GetOutput()->GetCellData()->SetScalars(line_scalars); line_source->GetOutput()->GetCellData()->SetScalars(line_scalars);
appendFilter->AddInputConnection(line_source->GetOutputPort()); appendFilter->AddInputConnection(line_source->GetOutputPort());
last_pos = new_pos;
} }
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New(); vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();