void RobotNode::updateTransformationMatrices()
{
if (this->getParent())
updateTransformationMatrices(this->getParent()->getGlobalPose());
else
{
// check for root
RobotPtr r = getRobot();
if (r && r->getRootNode() == shared_from_this())
updateTransformationMatrices(r->getGlobalPose());
else
updateTransformationMatrices(Eigen::Matrix4f::Identity());
}
}
void RobotNode::updatePose(bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
// apply propagated joint values
if (propagatedJointValues.size()>0)
{
RobotPtr r = robot.lock();
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it!=propagatedJointValues.end())
{
RobotNodePtr rn = r->getRobotNode(it->first);
if (!rn)
{
VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
} else
{
rn->setJointValue(jointValue*it->second);
}
it++;
}
}
}
void RobotNode::updatePose(const Eigen::Matrix4f &globalPose, bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices(globalPose);
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
void Template::setTransform(const TemplateTransform& transform)
{
Q_ASSERT(!is_georeferenced);
setTemplateAreaDirty();
this->transform = transform;
updateTransformationMatrices();
setTemplateAreaDirty();
map->setTemplatesDirty();
}
void Template::switchTransforms()
{
Q_ASSERT(!is_georeferenced);
setTemplateAreaDirty();
TemplateTransform temp = transform;
transform = other_transform;
other_transform = temp;
template_to_map_other = template_to_map;
updateTransformationMatrices();
adjusted = !adjusted;
setTemplateAreaDirty();
map->setTemplatesDirty();
}
bool Template::loadTemplateConfiguration(QIODevice* stream, int version)
{
loadString(stream, template_file);
if (version >= 27)
stream->read((char*)&is_georeferenced, sizeof(bool));
else
is_georeferenced = false;
if (!is_georeferenced)
{
transform.load(stream);
other_transform.load(stream);
stream->read((char*)&adjusted, sizeof(bool));
stream->read((char*)&adjustment_dirty, sizeof(bool));
int num_passpoints;
stream->read((char*)&num_passpoints, sizeof(int));
passpoints.resize(num_passpoints);
for (int i = 0; i < num_passpoints; ++i)
passpoints[i].load(stream, version);
map_to_template.load(stream);
template_to_map.load(stream);
template_to_map_other.load(stream);
stream->read((char*)&template_group, sizeof(int));
}
if (version >= 27)
{
if (!loadTypeSpecificTemplateConfiguration(stream, version))
return false;
}
else
{
// Adjust old file format version's templates
is_georeferenced = getTemplateType() == "TemplateTrack";
if (getTemplateType() == "TemplateImage")
{
transform.template_scale_x *= 1000.0 / map->getScaleDenominator();
transform.template_scale_y *= 1000.0 / map->getScaleDenominator();
updateTransformationMatrices();
}
}
return true;
}
void TemplateImage::updatePosFromGeoreferencing()
{
// Determine map coords of three image corner points
// by transforming the points from one Georeferencing into the other
bool ok;
MapCoordF top_left = map->getGeoreferencing().toMapCoordF(georef.data(), MapCoordF(-0.5, -0.5), &ok);
if (!ok)
{
qDebug() << "updatePosFromGeoreferencing() failed";
return; // TODO: proper error message?
}
MapCoordF top_right = map->getGeoreferencing().toMapCoordF(georef.data(), MapCoordF(image.width() - 0.5, -0.5), &ok);
if (!ok)
{
qDebug() << "updatePosFromGeoreferencing() failed";
return; // TODO: proper error message?
}
MapCoordF bottom_left = map->getGeoreferencing().toMapCoordF(georef.data(), MapCoordF(-0.5, image.height() - 0.5), &ok);
if (!ok)
{
qDebug() << "updatePosFromGeoreferencing() failed";
return; // TODO: proper error message?
}
// Calculate template transform as similarity transform from pixels to map coordinates
PassPointList pp_list;
PassPoint pp;
pp.src_coords = MapCoordF(-0.5 * image.width(), -0.5 * image.height());
pp.dest_coords = top_left;
pp_list.push_back(pp);
pp.src_coords = MapCoordF(0.5 * image.width(), -0.5 * image.height());
pp.dest_coords = top_right;
pp_list.push_back(pp);
pp.src_coords = MapCoordF(-0.5 * image.width(), 0.5 * image.height());
pp.dest_coords = bottom_left;
pp_list.push_back(pp);
QTransform q_transform;
if (!pp_list.estimateNonIsometricSimilarityTransform(&q_transform))
{
qDebug() << "updatePosFromGeoreferencing() failed";
return; // TODO: proper error message?
}
qTransformToTemplateTransform(q_transform, &transform);
updateTransformationMatrices();
}
Template::Template(const QString& path, Map* map) : map(map)
{
template_path = path;
if (! QFileInfo(path).canonicalFilePath().isEmpty())
template_path = QFileInfo(path).canonicalFilePath();
template_file = QFileInfo(path).fileName();
template_relative_path = "";
template_state = Unloaded;
has_unsaved_changes = false;
is_georeferenced = false;
adjusted = false;
adjustment_dirty = true;
template_group = -1;
updateTransformationMatrices();
}
bool TemplateImage::postLoadConfiguration(QWidget* dialog_parent, bool& out_center_in_view)
{
Q_UNUSED(out_center_in_view);
if (getTemplateFilename().endsWith(".gif", Qt::CaseInsensitive))
QMessageBox::warning(dialog_parent, tr("Warning"), tr("Loading a GIF image template.\nSaving GIF files is not supported. This means that drawings on this template won't be saved!\nIf you do not intend to draw on this template however, that is no problem."));
TemplateImageOpenDialog open_dialog(this, dialog_parent);
open_dialog.setWindowModality(Qt::WindowModal);
while (true)
{
if (open_dialog.exec() == QDialog::Rejected)
return false;
if (open_dialog.isGeorefRadioChecked() && !map->getGeoreferencing().isValid())
{
// Make sure that the map is georeferenced;
// use the center coordinates of the image as initial reference point.
calculateGeoreferencing();
QPointF template_coords_center = georef->toProjectedCoords(MapCoordF(0.5 * (image.width() - 1), 0.5 * (image.height() - 1)));
bool template_coords_probably_geographic =
template_coords_center.x() >= -90 && template_coords_center.x() <= 90 &&
template_coords_center.y() >= -90 && template_coords_center.y() <= 90;
Georeferencing initial_georef(map->getGeoreferencing());
if (template_coords_probably_geographic)
initial_georef.setGeographicRefPoint(LatLon(template_coords_center.y(), template_coords_center.x()));
else
initial_georef.setProjectedRefPoint(template_coords_center);
initial_georef.setState(Georeferencing::ScaleOnly);
GeoreferencingDialog dialog(dialog_parent, map, &initial_georef, false);
if (template_coords_probably_geographic)
dialog.setKeepGeographicRefCoords();
else
dialog.setKeepProjectedRefCoords();
if (dialog.exec() == QDialog::Rejected)
continue;
}
if (open_dialog.isGeorefRadioChecked() && available_georef == Georeferencing_WorldFile)
{
// Let user select the coordinate reference system, as this is not specified in world files
SelectCRSDialog dialog(map, dialog_parent, true, false, true, tr("Select the coordinate reference system of the coordinates in the world file"));
if (dialog.exec() == QDialog::Rejected)
continue;
temp_crs_spec = dialog.getCRSSpec();
}
break;
}
is_georeferenced = open_dialog.isGeorefRadioChecked();
if (is_georeferenced)
calculateGeoreferencing();
else
{
transform.template_scale_x = open_dialog.getMpp() * 1000.0 / map->getScaleDenominator();
transform.template_scale_y = transform.template_scale_x;
//transform.template_x = main_view->getPositionX();
//transform.template_y = main_view->getPositionY();
updateTransformationMatrices();
}
return true;
}
void Template::setTemplatePosition(MapCoord coord)
{
transform.template_x = coord.nativeX();
transform.template_y = coord.nativeY();
updateTransformationMatrices();
}
bool TemplateTrack::postLoadConfiguration(QWidget* dialog_parent, bool& out_center_in_view)
{
is_georeferenced = true;
// If no track CRS is given by the template file, ask the user
if (!track.hasTrackCRS())
{
if (map->getGeoreferencing().isLocal())
{
track_crs_spec.clear();
}
else
{
SelectCRSDialog dialog(
map->getGeoreferencing(),
dialog_parent,
SelectCRSDialog::TakeFromMap | SelectCRSDialog::Local | SelectCRSDialog::Geographic,
tr("Select the coordinate reference system of the track coordinates") );
if (dialog.exec() == QDialog::Rejected)
return false;
track_crs_spec = dialog.currentCRSSpec();
}
Georeferencing* track_crs = new Georeferencing();
if (!track_crs_spec.isEmpty())
track_crs->setProjectedCRS(QString{}, track_crs_spec);
track_crs->setTransformationDirectly(QTransform());
track.setTrackCRS(track_crs);
}
// If the CRS is geographic, ask if track should be loaded using map georeferencing or ad-hoc georeferencing
track_crs_spec = track.getTrackCRS()->getProjectedCRSSpec();
bool crs_is_geographic = track_crs_spec.contains(QLatin1String("+proj=latlong")); // TODO: should that be case insensitive?
if (crs_is_geographic)
{
TaskDialog georef_dialog(dialog_parent, tr("Opening track ..."),
tr("Load the track in georeferenced or non-georeferenced mode?"),
QDialogButtonBox::Abort);
QString georef_text = tr("Positions the track according to the map's georeferencing settings.");
if (!map->getGeoreferencing().isValid())
georef_text += QLatin1Char(' ') + tr("These are not configured yet, so they will be shown as the next step.");
QAbstractButton* georef_button = georef_dialog.addCommandButton(tr("Georeferenced"), georef_text);
QAbstractButton* non_georef_button = georef_dialog.addCommandButton(tr("Non-georeferenced"), tr("Projects the track using an orthographic projection with center at the track's coordinate average. Allows adjustment of the transformation and setting the map georeferencing using the adjusted track position."));
georef_dialog.exec();
if (georef_dialog.clickedButton() == georef_button)
is_georeferenced = true;
else if (georef_dialog.clickedButton() == non_georef_button)
is_georeferenced = false;
else // abort
return false;
}
// If the CRS is local, show positioning dialog
if (track_crs_spec.isEmpty())
{
is_georeferenced = false;
TemplatePositioningDialog dialog(dialog_parent);
if (dialog.exec() == QDialog::Rejected)
return false;
transform.template_scale_x = dialog.getUnitScale();
if (!dialog.useRealCoords())
{
transform.template_scale_x /= (map->getScaleDenominator() / 1000.0);
}
transform.template_scale_y = transform.template_scale_x;
updateTransformationMatrices();
out_center_in_view = dialog.centerOnView();
}
// If the track is loaded as georeferenced and the transformation parameters
// were not set yet, it must be done now
if (is_georeferenced &&
(!map->getGeoreferencing().isValid() || map->getGeoreferencing().isLocal()))
{
// Set default for real world reference point as some average of the track coordinates
Georeferencing georef(map->getGeoreferencing());
georef.setGeographicRefPoint(track.calcAveragePosition());
// Show the parameter dialog
GeoreferencingDialog dialog(dialog_parent, map, &georef);
dialog.setKeepGeographicRefCoords();
if (dialog.exec() == QDialog::Rejected || map->getGeoreferencing().isLocal())
return false;
}
// If the track is loaded as not georeferenced,
// the map coords for the track coordinates have to be calculated
if (!is_georeferenced && crs_is_geographic)
calculateLocalGeoreferencing();
else
track.changeMapGeoreferencing(map->getGeoreferencing());
return true;
}
