// Performs one round of validation on the given bin, after training
double checkBin(unsigned bin, const std::vector<Examples>& sets,
const std::vector<unsigned>& topology, unsigned total)
{
NeuralNet network(topology);
std::vector<Example> toTrainOn;
reconcile(sets, toTrainOn, bin);
// Train on training set
Transformation t = getTransformation(toTrainOn);
applyTransformation(toTrainOn, t, false);
network.train(toTrainOn, 1.0, 0.001, 4000 * total);
// Compute average error in validation set, unscaled
const std::vector<Example>& validation = sets[bin];
double error;
for(std::size_t i = 0; i < validation.size(); i++) {
const Example& original = validation[i];
network.computeActivation(original.input);
Output netOut = network.getActivation();
applyTransformation(netOut, t, true);
for(std::size_t j = 0; j < netOut.values.size(); j++) {
error += std::fabs(netOut.values[j] - original.output.values[j]);
}
}
return error / validation.size();
}
void TriangleMesh::transform(double xfov, double aspectRatio, double znear, double zfar) {
vector<Point3> points;
vector<double> pointsw;
vector<Vector3> normals;
vector<double> normalsw;
vector<unsigned int> indices;
for (size_t i = 0; i < posArray.getCount(); i += 4) {
points.push_back(Point3(posArray[i], posArray[i+1], posArray[i+2]));
pointsw.push_back(posArray[i+3]);
}
for (size_t i = 0; i < normalArray.getCount(); i += 4) {
normals.push_back(Vector3(normalArray[i], normalArray[i+1], normalArray[i+2]));
normalsw.push_back(normalArray[i+3]);
}
for (size_t i = 0; i < inds.getCount(); i++)
indices.push_back(inds[i]);
applyTransformation(xfov, aspectRatio, znear, zfar, &points, &pointsw, &normals, &normalsw, &indices);
for (size_t i = 0; i < posArray.getCount(); i += 4) {
posArray[i] = points[i/4].x;
posArray[i+1] = points[i/4].y;
posArray[i+2] = points[i/4].z;
posArray[i+3] = pointsw[i/4];
}
for (size_t i = 0; i < normalArray.getCount(); i += 4) {
normalArray[i] = normals[i/4].x;
normalArray[i+1] = normals[i/4].y;
normalArray[i+2] = normals[i/4].z;
normalArray[i+3] = normalsw[i/4];
}
for (size_t i = 0; i < inds.getCount(); i++)
inds[i] = indices[i];
}
void MoveTool::beginInteraction(Qt::KeyboardModifiers keyMod, Layer* layer)
{
QRectF selectionRect = mScribbleArea->myTransformedSelection;
if (!selectionRect.isNull())
{
mEditor->backup(typeName());
}
mScribbleArea->findMoveModeOfCornerInRange();
mScribbleArea->myRotatedAngle = mRotatedAngle;
if (keyMod != Qt::ShiftModifier)
{
if (shouldDeselect())
{
applyTransformation();
mScribbleArea->deselectAll();
}
}
if (mScribbleArea->getMoveMode() == MoveMode::MIDDLE)
{
if (keyMod == Qt::ControlModifier) // --- rotation
{
mScribbleArea->setMoveMode(MoveMode::ROTATION);
}
}
if (layer->type() == Layer::VECTOR)
{
createVectorSelection(keyMod, layer);
}
}
bool MoveTool::switchingLayer()
{
if (!transformHasBeenModified())
{
mScribbleArea->deselectAll();
return true;
}
int returnValue = showTransformWarning();
if (returnValue == QMessageBox::Yes)
{
if (mCurrentLayer->type() == Layer::BITMAP)
{
applySelectionChanges();
}
else if (mCurrentLayer->type() == Layer::VECTOR)
{
applyTransformation();
}
mScribbleArea->deselectAll();
return true;
}
else if (returnValue == QMessageBox::No)
{
cancelChanges();
return true;
}
else if (returnValue == QMessageBox::Cancel)
{
return false;
}
return true;
}
void TriangleMesh::transform(CLIP clipSide, double xfov, double aspectRatio, double znear, double zfar) {
vector<Point3> points;
vector<double> pointsw;
vector<Vector3> normals;
vector<double> normalsw;
vector<unsigned int> indices;
for (size_t i = 0; i < posArray.getCount(); i += 4) {
points.push_back(Point3(posArray[i], posArray[i+1], posArray[i+2]));
pointsw.push_back(posArray[i+3]);
}
for (size_t i = 0; i < normalArray.getCount(); i += 4) {
normals.push_back(Vector3(normalArray[i], normalArray[i+1], normalArray[i+2]));
normalsw.push_back(normalArray[i+3]);
}
for (size_t i = 0; i < inds.getCount(); i++)
indices.push_back(inds[i]);
applyTransformation(clipSide, xfov, aspectRatio, znear, zfar, &points, &pointsw, &normals, &normalsw, &indices);
posArray.clear();
normalArray.clear();
inds.clear();
for (size_t i = 0; i < points.size(); i++)
posArray.append4(points[i].x, points[i].y, points[i].z, pointsw[i]);
for (size_t i = 0; i < normals.size(); i++)
normalArray.append4(normals[i].x, normals[i].y, normals[i].z, normalsw[i]);
for (size_t i = 0; i < indices.size(); i++)
inds.append(indices[i]);
}
bool applyProjectionToOneField(StringData field) const {
MutableDocument doc;
const FieldPath f{field};
doc.setNestedField(f, Value(1.0));
const Document transformedDoc = applyTransformation(doc.freeze());
return !transformedDoc.getNestedField(f).missing();
}
void SurfaceAnd::transform(const Transform& transform)
{
blockUpdates();
for(auto& surf : _surfs)
applyTransformation(surf, transform);
unblockUpdates();
}
EnumIcpState Icp::step(double* rms, unsigned int* pairs)
{
Timer t;
if(_model==NULL || _sceneTmp == NULL) return ICP_ERROR;
EnumIcpState retval = ICP_PROCESSING;
vector<StrCartesianIndexPair>* pvPairs;
_estimator->setScene(_sceneTmp, _sizeScene, _normalsSTmp);
_assigner->determinePairs(_sceneTmp, _sizeScene);
pvPairs = _assigner->getPairs();
*pairs = pvPairs->size();
if(_trace)
{
_trace->addAssignment(_sceneTmp, _sizeScene, *pvPairs);
}
if(pvPairs->size()>2)
{
// Estimate transformation
_estimator->setPairs(pvPairs);
// get mapping error
*rms = _estimator->getRMS();
// estimate transformation
_estimator->estimateTransformation(_Tlast);
applyTransformation(_sceneTmp, _sizeScene, _dim, _Tlast);
if(_normalsS)
applyTransformation(_normalsSTmp, _sizeScene, _dim, _Tlast);
// update overall transformation
(*_Tfinal4x4) = (*_Tlast) * (*_Tfinal4x4);
}
else
{
retval = ICP_NOTMATCHABLE;
}
return retval;
}
void Transformation::applyTransformation(Sint16 &x, Sint16 &y) {
SDL_Point p;
p.x = x;
p.y = y;
applyTransformation(p);
x = p.x;
y = p.y;
}
void Transformation::applyTransformation(int &x, int &y) {
SDL_Point p;
p.x = x;
p.y = y;
applyTransformation(p);
x = p.x;
y = p.y;
}
EnumIcpState Icp::iterate(double* rms, unsigned int* pairs, unsigned int* iterations, Matrix* Tinit)
{
if(_trace)
{
_trace->reset();
_trace->setModel(_model, _sizeModel);
}
_Tfinal4x4->setIdentity();
if(Tinit)
{
applyTransformation(_sceneTmp, _sizeScene, _dim, Tinit);
if(_normalsSTmp) applyTransformation(_normalsSTmp, _sizeScene, _dim, Tinit);
(*_Tfinal4x4) = (*Tinit) * (*_Tfinal4x4);
}
EnumIcpState eRetval = ICP_PROCESSING;
unsigned int iter = 0;
double rms_prev = 10e12;
unsigned int conv_cnt = 0;
while( eRetval == ICP_PROCESSING )
{
eRetval = step(rms, pairs);
iter++;
if(fabs(*rms-rms_prev) < 10e-10)
conv_cnt++;
else
conv_cnt = 0;
if((*rms <= _maxRMS || conv_cnt>=_convCnt ))
eRetval = ICP_SUCCESS;
else if(iter >= _maxIterations)
eRetval = ICP_MAXITERATIONS;
rms_prev = *rms;
}
*iterations = iter;
return eRetval;
}
void MoveTool::setCurveSelected(VectorImage* vectorImage, Qt::KeyboardModifiers keyMod)
{
if (!vectorImage->isSelected(mScribbleArea->mClosestCurves))
{
if (keyMod != Qt::ShiftModifier)
{
applyTransformation();
}
vectorImage->setSelected(mScribbleArea->mClosestCurves, true);
mScribbleArea->setSelection(vectorImage->getSelectionRect());
}
}
void MoveTool::setAreaSelected(VectorImage* vectorImage, Qt::KeyboardModifiers keyMod)
{
int areaNumber = vectorImage->getLastAreaNumber(getLastPoint());
if (!vectorImage->isAreaSelected(areaNumber))
{
if (keyMod != Qt::ShiftModifier)
{
applyTransformation();
}
vectorImage->setAreaSelected(areaNumber, true);
mScribbleArea->setSelection(vectorImage->getSelectionRect());
}
}
bool MoveTool::leavingThisTool()
{
if (mCurrentLayer)
{
switch (mCurrentLayer->type())
{
case Layer::BITMAP: applySelectionChanges(); break;
case Layer::VECTOR: applyTransformation(); break;
default: break;
}
}
return true;
}
void scene_node::draw(Renderer* r, bool isTransparentPass) {
extern MatrixStack* sceneGraphMatrixStack;
sn_State* currentState = &sn_states[SimState::currentRenderState];
applyTransformation();
for(std::vector<scene_node*>::iterator it = currentState->children.begin(); it != currentState->children.end(); ++it) {
(*it)->draw(r, isTransparentPass);
}
sceneGraphMatrixStack->popMatrix();
}
void ArtisticTextShape::setSize( const QSizeF &newSize )
{
QSizeF oldSize = size();
if ( !oldSize.isNull() ) {
qreal zoomX = newSize.width() / oldSize.width();
qreal zoomY = newSize.height() / oldSize.height();
QTransform matrix( zoomX, 0, 0, zoomY, 0, 0 );
update();
applyTransformation( matrix );
update();
}
KoShape::setSize(newSize);
}
void MeshNode::draw(Renderer* r, bool isTransparentPass) {
extern MatrixStack* sceneGraphMatrixStack;
extern MatrixStack* projectionMatrixStack;
//if (glm::isIdentity(projectionMatrixStack->last(), 0.01f)) {
// std::cout << "projection matrix shouldn't be identity :/" << std::endl;
//}
mn_State* currentState = &mn_states[SimState::currentRenderState];
sn_State* currentSNState = &sn_states[SimState::currentRenderState];
bool isMaterialTransparent = currentState->material->getHasTransparency();
applyTransformation();
if (isMaterialTransparent == isTransparentPass) {
currentState->material->use();
//construct normal matrix
glm::mat4 normalMatrix = glm::mat4(sceneGraphMatrixStack->last());
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
//apply geometry/mesh uniforms
glUniformMatrix4fv(0, 1, GL_FALSE, glm::value_ptr(sceneGraphMatrixStack->last()));
glUniformMatrix4fv(1, 1, GL_FALSE, glm::value_ptr(projectionMatrixStack->last()));
glUniformMatrix4fv(2, 1, GL_FALSE, glm::value_ptr(normalMatrix));
//apply material uniforms
currentState->material->setUniforms(currentState->properties);
//bind, draw, and unbind vertices
currentState->objectMesh->bindBuffers();
glDrawElements(GL_TRIANGLES, 3*currentState->objectMesh->getTriCount(), GL_UNSIGNED_INT, NULL);
currentState->objectMesh->unbindBuffers();
currentState->material->unuse();
}
//draw children
for(std::vector<scene_node*>::iterator it = currentSNState->children.begin(); it != currentSNState->children.end(); ++it) {
(*it)->draw(r, isTransparentPass);
}
//undo changes to matrix
sceneGraphMatrixStack->popMatrix();
}
void CglicScene::display()
{
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_CULL_FACE);
if(pcv->profile.globalScale){
for(int i = 0 ; i < listObject.size() ; i++){
listObject[i]->setScaleFactor(globalScale);
}
}
update_matrices();
applyTransformation();
for (int i = 0; i < listGroup.size(); i++)
listGroup[i]->compute();
for (int iObj = 0; iObj < listObject.size(); iObj++)
listObject[iObj]->applyTransformation();
axis->applyTransformation();
axis->display();
for (int iObj = 0; iObj < listObject.size(); iObj++)
if(!listObject[iObj]->isHidden())
listObject[iObj]->shadowsDisplay();
for (int iObj = 0; iObj < listObject.size(); iObj++)
if(listObject[iObj]->isHidden())
listObject[iObj]->shadowsDisplay();
//Display de l'ensemble des artefacts
for (int iObj = 0; iObj < listObject.size(); iObj++)
listObject[iObj]->artifactsDisplay();
//Display des meshes
for (int iObj = 0; iObj < listObject.size(); iObj++)
if(!listObject[iObj]->isHidden())
listObject[iObj]->display();
for (int iObj = 0; iObj < listObject.size(); iObj++)
if(listObject[iObj]->isHidden())
listObject[iObj]->display();
glClear(GL_DEPTH_BUFFER_BIT);
debug();
}
// Fitness between two volumes --------------------------------------------
double fitness(double *p)
{
applyTransformation(params.V2(),params.Vaux(),p);
// Correlate
double fit=0.;
switch (params.alignment_method)
{
case (COVARIANCE):
fit = -correlationIndex(params.V1(), params.Vaux(), params.mask_ptr);
break;
case (LEAST_SQUARES):
fit = rms(params.V1(), params.Vaux(), params.mask_ptr);
break;
}
return fit;
}
// private methods
//
void ManipulationBase::guiEventHandler() {
tgt::vec3 offset(0.0, 0.0, 0.0);
tgt::mat4 transformMatrix = tgt::mat4::createIdentity();
if (manipulationType_.get() == "move") {
float offsetSize = 10;
if (invertDirection_.get() == true) offsetSize = -offsetSize;
/**/ if (manipulationAxis_.get() == "x") offset[0] += offsetSize;
else if (manipulationAxis_.get() == "y") offset[1] += offsetSize;
else if (manipulationAxis_.get() == "z") offset[2] += offsetSize;
}
else if (manipulationType_.get() == "rotate") {
float angle = 10.f * 3.141592f / 180.f;
if (invertDirection_.get() == true) angle = - angle;
float sina = sin(angle);
float cosa = cos(angle);
/**/ if (manipulationAxis_.get() == "x")
transformMatrix = tgt::mat4(
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, cosa,-sina, 0.0f,
0.0f, sina, cosa, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
else if (manipulationAxis_.get() == "y")
transformMatrix = tgt::mat4(
cosa, 0.0f,-sina, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
sina, 0.0f, cosa, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
else if (manipulationAxis_.get() == "z")
transformMatrix = tgt::mat4(
cosa,-sina, 0.0f, 0.0f,
sina, cosa, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
}
applyTransformation(offset, transformMatrix);
}
void Kinect::update() {
plane.setFrom3Points(planeCoords[0], planeCoords[1], planeCoords[2]);
// do the ofNode stuff
applyTransformation();
// actual processing
if(running) {
kinect.update();
hasCreatedVertexArrays = false;
doVision();
}
}
int main(int argc, char **argv) {
if (argc < 5) {
std::cout << "Usage: "
<< argv[0]
<< " path_to_scans/ output.ply icp_iters subsample_probability" << std::endl;
return 1;
}
// Command line parameters
string pc_filepath = argv[1];
string pc_file_out_ply = argv[2];
int icp_num_iters = std::atoi(argv[3]);
double probability = std::atof(argv[4]);
if (pc_filepath.c_str()[pc_filepath.length() - 1] != '/') {
pc_filepath += "/";
}
std::ifstream file(pc_filepath + "config.txt");
string line;
getline(file, line);
std::istringstream in(line);
int num_images;
in >> num_images;
Mat pc_a = load_kinect_frame(pc_filepath + "image_0.png",
pc_filepath + "depth_0.txt");
Mat allSamples = selectRandomPoints(pc_a, probability);
// Used for accumulating the rigid transformation matrix
Mat transformation = Mat::eye(4, 4, CV_32F);
Mat rotation, translation;
clock_t time;
for (int image_num = 1; image_num < num_images; ++image_num) {
std::cout << "REGISTERING IMAGE " << image_num << std::endl;
time = clock();
// Load the point cloud to be registered
string str_num = std::to_string(image_num);
Mat pc_b = load_kinect_frame(pc_filepath + "image_" + str_num + ".png",
pc_filepath + "depth_" + str_num + ".txt");
Mat pc_b_sample = selectRandomPoints(pc_b, probability);
// Apply the previous transformations to b so that it is positioned near
// the last accumulated points
extractRigidTransform(transformation, rotation, translation);
pc_b_sample = applyTransformation(pc_b_sample, rotation, translation);
pc_b = applyTransformation(pc_b, rotation, translation);
// Perform the specified number of icp iterations
for (int i = 0; i < icp_num_iters; ++i) {
// Find the nearest neighbor pairs in the two point clouds
Mat a, b;
nearest_neighbors(allSamples, pc_b_sample, a, b);
// Find the optimal rotation and translation matrix to transform b onto a
Mat r, t;
rigid_transform_3D(b, a, r, t);
// Apply the rigid transformation to the b point cloud
pc_b_sample = applyTransformation(pc_b_sample, r, t);
pc_b = applyTransformation(pc_b, r, t);
transformation *= getRigidTransform(r, t);
}
// Combine the two point clouds and save to disk
Mat combined, combinedSample;
vconcat(pc_a, pc_b, combined);
vconcat(allSamples, pc_b_sample, combinedSample);
pc_a = combined;
allSamples = combinedSample;
save_point_cloud(combined, pc_file_out_ply);
std::cout << "complete " << ((float)(clock() - time)) / CLOCKS_PER_SEC << std::endl;
}
return 0;
}
intensityCloud::Ptr ICP::getTransformation(avora_msgs::StampedIntensityCloudPtr P0, MLSM *X, Vector3d eV, Vector3d eT, Matrix4f eR, Vector3d *Tv, Vector3d *T, Matrix4f *R){
double error = DBL_MAX;
unsigned int iterations = 0;
std::vector<BlockInfo> Y;
intensityCloud cloud, cloud0;
pcl::fromROSMsg(P0->cloud,cloud);
pcl::fromROSMsg(P0->cloud,cloud0);
std::vector<Vector3d> transforms(cloud.size());
intensityCloud::Ptr P = boost::make_shared<intensityCloud>(cloud);
// Initial transform
ROS_INFO("Applying initial transformation");
Vector3d accumulatedT;
Vector3d accumulatedTv;
Matrix4f accumulatedR = Matrix<float, 4, 4>::Identity();
accumulatedT[0] = 0;
accumulatedT[1] = 0;
accumulatedT[2] = 0;
accumulatedTv[0] = 0;//eV[0];
accumulatedTv[1] = 0;//eV[1];
accumulatedTv[2] = 0;//eV[2];
(*Tv)[0] = 0;//eV[0];
(*Tv)[1] = 0;//eV[1];
(*Tv)[2] = 0;//eV[2];
//std::vector<Vector3d> transforms = applyTransformation(P, P0->timeStamps, eR, eV, eT);
ROS_INFO("Initial transformation applied");
//intensityCloud sum;
sensor_msgs::PointCloud2 debugCloud;
//sum = cloud0 + *P;
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(5);
while ((iterations < maxIterations_) && (error > errorThreshold_)){
(*R) = Matrix<float, 4, 4>::Identity();
// Calculate closest points
ROS_INFO("Get closest points");
Y = closestPoints(P,X, P0->timeStamps,eV);
// Calculate transformation
ROS_INFO("Get transformation");
error = registration(P,&Y,P0->timeStamps,&transforms, T, R,eV);
// Iterate through P applying the correct transformation depending on Tv and the stamp
ROS_INFO("Applying transformation");
//transforms = applyTransformation(P,P0->timeStamps, *R, *Tv, *T);
applyTransformation(P, *R, transforms);
// Calculate error
ROS_INFO("Calculate error");
error = calculateError(P, &Y);
//ROS_INFO("%d error = %f",iterations, error);
// Iterate?
iterations++;
accumulatedT += *T;
//accumulatedT += transforms.back();
//accumulatedTv += *Tv;
//ROS_INFO("\n%d \tAcc Vel x = %f y = %f z = %f error = %f",iterations,accumulatedTv[0], accumulatedTv[1], accumulatedTv[2], error);
accumulatedR = accumulatedR * (*R);
ROS_INFO("\n%d \tTrl x = %f y = %f z = %f error = %f \n\tAcc x = %f y = %f z = %f",iterations,(*T)[0], (*T)[1], (*T)[2], error,
accumulatedT[0], accumulatedT[1], accumulatedT[2]);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(1);
}
pcl::toROSMsg(*P,debugCloud);
debugCloud.header.frame_id = "map";
debugPublisher_->publish(debugCloud);
//(*T)[0] = //(*Tv)[0] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[1] = //(*Tv)[1] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[2] = //(*Tv)[2] * (P0->timeStamps.back() - P0->timeStamps.front());
(*T) = accumulatedT;
(*R) = accumulatedR;
ROS_INFO("Scan time: %f",fabs(P0->timeStamps.back()) - fabs(P0->timeStamps.front()));
ROS_INFO("\n%d \tMoved x = %f y = %f z = %f error = %f ",iterations,(*T)[0], (*T)[1], (*T)[2], error);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//(*T) = accumulatedT;
return P;
//return error < errorThreshold_;
}
void SurfaceGhost::transform(const Transform& transform)
{
applyTransformation(_surf, transform);
}
void SurfaceInverse::transform(const Transform& transform)
{
applyTransformation(_surf, transform);
}
void run ()
{
mask.allowed_data_types = INT_MASK;
// Main program =========================================================
params.V1.read(fn1);
params.V1().setXmippOrigin();
params.V2.read(fn2);
params.V2().setXmippOrigin();
// Initialize best_fit
double best_fit = 1e38;
Matrix1D<double> best_align(8);
bool first = true;
// Generate mask
if (mask_enabled)
{
mask.generate_mask(params.V1());
params.mask_ptr = &(mask.get_binary_mask());
}
else
params.mask_ptr = NULL;
// Exhaustive search
if (!usePowell && !useFRM)
{
// Count number of iterations
int times = 1;
if (!tell)
{
if (grey_scale0 != grey_scaleF)
times *= FLOOR(1 + (grey_scaleF - grey_scale0) / step_grey);
if (grey_shift0 != grey_shiftF)
times *= FLOOR(1 + (grey_shiftF - grey_shift0) / step_grey_shift);
if (rot0 != rotF)
times *= FLOOR(1 + (rotF - rot0) / step_rot);
if (tilt0 != tiltF)
times *= FLOOR(1 + (tiltF - tilt0) / step_tilt);
if (psi0 != psiF)
times *= FLOOR(1 + (psiF - psi0) / step_psi);
if (scale0 != scaleF)
times *= FLOOR(1 + (scaleF - scale0) / step_scale);
if (z0 != zF)
times *= FLOOR(1 + (zF - z0) / step_z);
if (y0 != yF)
times *= FLOOR(1 + (yF - y0) / step_y);
if (x0 != xF)
times *= FLOOR(1 + (xF - x0) / step_x);
init_progress_bar(times);
}
else
std::cout << "#grey_factor rot tilt psi scale z y x fitness\n";
// Iterate
int itime = 0;
int step_time = CEIL((double)times / 60.0);
Matrix1D<double> r(3);
Matrix1D<double> trial(9);
for (double grey_scale = grey_scale0; grey_scale <= grey_scaleF ; grey_scale += step_grey)
for (double grey_shift = grey_shift0; grey_shift <= grey_shiftF ; grey_shift += step_grey_shift)
for (double rot = rot0; rot <= rotF ; rot += step_rot)
for (double tilt = tilt0; tilt <= tiltF ; tilt += step_tilt)
for (double psi = psi0; psi <= psiF ; psi += step_psi)
for (double scale = scale0; scale <= scaleF ; scale += step_scale)
for (ZZ(r) = z0; ZZ(r) <= zF ; ZZ(r) += step_z)
for (YY(r) = y0; YY(r) <= yF ; YY(r) += step_y)
for (XX(r) = x0; XX(r) <= xF ; XX(r) += step_x)
{
// Form trial vector
trial(0) = grey_scale;
trial(1) = grey_shift;
trial(2) = rot;
trial(3) = tilt;
trial(4) = psi;
trial(5) = scale;
trial(6) = ZZ(r);
trial(7) = YY(r);
trial(8) = XX(r);
// Evaluate
double fit = fitness(MATRIX1D_ARRAY(trial));
// The best?
if (fit < best_fit || first)
{
best_fit = fit;
best_align = trial;
first = false;
if (tell)
std::cout << "Best so far\n";
}
// Show fit
if (tell)
std::cout << trial << " " << fit << std::endl;
else
if (++itime % step_time == 0)
progress_bar(itime);
}
if (!tell)
progress_bar(times);
}
else if (usePowell)
{
// Use Powell optimization
Matrix1D<double> x(9), steps(9);
double fitness;
int iter;
steps.initConstant(1);
if (onlyShift)
steps(0)=steps(1)=steps(2)=steps(3)=steps(4)=steps(5)=0;
if (params.alignment_method == COVARIANCE)
steps(0)=steps(1)=0;
x(0)=grey_scale0;
x(1)=grey_shift0;
x(2)=rot0;
x(3)=tilt0;
x(4)=psi0;
x(5)=scale0;
x(6)=z0;
x(7)=y0;
x(8)=x0;
powellOptimizer(x,1,9,&wrapperFitness,NULL,0.01,fitness,iter,steps,true);
best_align=x;
best_fit=fitness;
first=false;
}
else if (useFRM)
{
String scipionPython;
initializeScipionPython(scipionPython);
PyObject * pFunc = getPointerToPythonFRMFunction();
double rot,tilt,psi,x,y,z,score;
Matrix2D<double> A;
alignVolumesFRM(pFunc, params.V1(), params.V2(), Py_None, rot,tilt,psi,x,y,z,score,A,maxShift,maxFreq,params.mask_ptr);
best_align.initZeros(9);
best_align(0)=1; // Gray scale
best_align(1)=0; // Gray shift
best_align(2)=rot;
best_align(3)=tilt;
best_align(4)=psi;
best_align(5)=1; // Scale
best_align(6)=z;
best_align(7)=y;
best_align(8)=x;
best_fit=-score;
}
if (!first)
std::cout << "The best correlation is for\n"
<< "Scale : " << best_align(5) << std::endl
<< "Translation (X,Y,Z) : " << best_align(8)
<< " " << best_align(7) << " " << best_align(6)
<< std::endl
<< "Rotation (rot,tilt,psi): "
<< best_align(2) << " " << best_align(3) << " "
<< best_align(4) << std::endl
<< "Best grey scale : " << best_align(0) << std::endl
<< "Best grey shift : " << best_align(1) << std::endl
<< "Fitness value : " << best_fit << std::endl;
Matrix1D<double> r(3);
XX(r) = best_align(8);
YY(r) = best_align(7);
ZZ(r) = best_align(6);
Matrix2D<double> A,Aaux;
Euler_angles2matrix(best_align(2), best_align(3), best_align(4),
A, true);
translation3DMatrix(r,Aaux);
A = A * Aaux;
scale3DMatrix(vectorR3(best_align(5), best_align(5), best_align(5)),Aaux);
A = A * Aaux;
if (verbose!=0)
std::cout << "xmipp_transform_geometry will require the following values"
<< "\n Angles: " << best_align(2) << " "
<< best_align(3) << " " << best_align(4)
<< "\n Shifts: " << A(0,3) << " " << A(1,3) << " " << A(2,3)
<< std::endl;
if (apply)
{
applyTransformation(params.V2(),params.Vaux(),MATRIX1D_ARRAY(best_align));
params.V2()=params.Vaux();
params.V2.write(fnOut);
}
}
SDL_Point Transformation::transform(SDL_Point p){
applyTransformation(p);
return p;
}
BSONObj applyProjection(BSONObj inputDoc) const {
return applyTransformation(Document{inputDoc}).toBson();
}
void BlendArmAnimation::update(float dt)
{
gAction(currentAction).update(dt);
applyTransformation();
}