自学教程：C++ translation函数代码示例

vqs vqs::operator+( const vqs&	pVQS ){	CVector3 transAdd = translation() + pVQS.translation();	DQuaternion quatAdd = orientation() + pVQS.orientation();	float scaleAdd = scale() + pVQS.scale(); 	vqs result;	result.Set(transAdd, quatAdd, scaleAdd);	return result;}

// Position and Rotationconst LLMatrix4&  	LLMatrix4::initRotTrans(const F32 angle, const F32 rx, const F32 ry, const F32 rz,											const F32 tx, const F32 ty, const F32 tz){	LLMatrix3	mat(angle, rx, ry, rz);	LLVector3	translation(tx, ty, tz);	initMatrix(mat);	setTranslation(translation);	return (*this);}

/*!    Returns the transformation to apply to the modelview matrix    to present the scene as viewed from the eye position.    The /a eye parameter is used to adjust the camera's position    horizontally by half of eyeSeparation() if /a eye is QGL::LeftEye    or QGL::RightEye.    /sa projectionMatrix()*/QMatrix4x4 QGLCamera::modelViewMatrix(QGL::Eye eye) const{    Q_D(const QGLCamera);    QMatrix4x4 m;    QVector3D adjust;    if (eye == QGL::LeftEye)        adjust = translation(-d->eyeSeparation / 2.0f, 0.0f, 0.0f);    else if (eye == QGL::RightEye)        adjust = translation(d->eyeSeparation / 2.0f, 0.0f, 0.0f);    if (d->motionQuaternion.isIdentity()) {        m.lookAt(d->eye + adjust, d->center, d->upVector);    } else {        QVector3D up = d->motionQuaternion.rotatedVector(d->upVector);        QVector3D view = d->motionQuaternion.rotatedVector(d->viewVector);        m.lookAt(d->center - view + adjust, d->center, up);    }    return m;}

void RotateGizmoRing::render(IViewer* viewer) const {  GraphicsInterface::beginPerformanceEvent("Ring");  GraphicsInterface::resetRenderTarget(true);  GraphicsInterface::setViewport(GraphicsInterface::backBufferSize());  GraphicsInterface::setBlendState(IGraphicsInterface::NOBLEND);  GraphicsInterface::setRenderState(CULL_MODE_NONE, true);  if (drawBounds_)  {    effect_->beginDraw();    BoundingBox bounds = boundingBox();    Vector4 scale(bounds.max.x - bounds.min.x, bounds.max.y - bounds.min.y, bounds.max.z - bounds.min.z, 2.0f);    scale /= 2.0f;    Vector4 translation(bounds.max.x + bounds.min.x, bounds.max.y + bounds.min.y, bounds.max.z + bounds.min.z, 2.0f);    translation /= 2.0f;    Matrix4x4 model = localToWorld() * Matrix4x4::translation(translation) * Matrix4x4::scale(scale);    Matrix4x4 modelViewProjection = viewer->projection() * viewer->viewTransform() * model * Matrix4x4::scale(1.0001f);    effect_->setUniform(modelViewProjection, "ModelViewProj");    effect_->setUniform(ringColor_, "Color");    effect_->commitBuffers();    GraphicsInterface::drawVertexBuffer(Geometry::UNIT_CUBE_VERTEX_BUFFER, Geometry::UNIT_CUBE_VERTEX_COUNT, Geometry::UNIT_CUBE_VERTEX_FORMAT);    effect_->endDraw();  }  // ring  {     effect_->beginDraw();    Color3 color = ringColor_;    if (isHighlighted_) {      color = Color3::YELLOW;    }    effect_->setUniform(color, "Color");    Matrix4x4 modelViewProjection = viewer->projection() * viewer->viewTransform() * localToWorld();    effect_->setUniform(modelViewProjection, "ModelViewProj");        Matrix4x4 model = localToWorld();    effect_->setUniform(model, "Model");    effect_->commitBuffers();    GraphicsInterface::drawVertexBuffer(Geometry::UNIT_CIRCLE_VERTEX_BUFFER, Geometry::UNIT_CIRCLE_VERTEX_COUNT, Geometry::UNIT_CIRCLE_VERTEX_FORMAT);    effect_->endDraw();  }  GraphicsInterface::endPerformanceEvent();}

void GeneralSettingsPage::apply(){	auto language = language_box->currentData();	if (language != getSetting(Settings::General_Language)	    || translation_file != getSetting(Settings::General_TranslationFile).toString())	{		// Show an message box in the new language.		TranslationUtil translation((QLocale::Language)language.toInt(), translation_file);		auto new_language = translation.getLocale().language();		switch (new_language)		{		case QLocale::AnyLanguage:		case QLocale::C:		case QLocale::English:			QMessageBox::information(window(), QLatin1String("Notice"), QLatin1String("The program must be restarted for the language change to take effect!"));			break;					default:			qApp->installEventFilter(this);			qApp->installTranslator(&translation.getQtTranslator());			qApp->installTranslator(&translation.getAppTranslator());			QMessageBox::information(window(), tr("Notice"), tr("The program must be restarted for the language change to take effect!"));			qApp->removeTranslator(&translation.getAppTranslator());			qApp->removeTranslator(&translation.getQtTranslator());			qApp->removeEventFilter(this);		}				setSetting(Settings::General_Language, new_language);		setSetting(Settings::General_TranslationFile, translation_file);#if defined(Q_OS_MAC)		// The native [file] dialogs will use the first element of the		// AppleLanguages array in the application's .plist file -		// and this file is also the one used by QSettings.		const QString mapper_language(translation.getLocale().name().left(2));		QSettings().setValue(QLatin1String{"AppleLanguages"}, { mapper_language });#endif	}		setSetting(Settings::General_OpenMRUFile, open_mru_check->isChecked());	setSetting(Settings::HomeScreen_TipsVisible, tips_visible_check->isChecked());	setSetting(Settings::General_NewOcd8Implementation, ocd_importer_check->isChecked());	setSetting(Settings::General_RetainCompatiblity, compatibility_check->isChecked());	setSetting(Settings::General_PixelsPerInch, ppi_edit->value());		auto name_latin1 = encoding_box->currentText().toLatin1();	if (name_latin1 == "System"	    || QTextCodec::codecForName(name_latin1))	{		setSetting(Settings::General_Local8BitEncoding, name_latin1);	}		int interval = autosave_interval_edit->value();	if (!autosave_check->isChecked())		interval = -interval;	setSetting(Settings::General_AutosaveInterval, interval);}

void MapBuilder::onCloudFrame2(pcl::PointCloud<pcl::PointXYZ>::Ptr frame, pcl::PointXY sensorOffset, RobotPosition pose){    //Transform current cloud to match robot's position    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);    double mx =  pose.X + sensorOffset.x;    double my =  pose.Y + sensorOffset.y;    double mt = AngleUtils::degToRads(pose.Heading);    Eigen::Vector3f translation(mx, my, 0);    Eigen::Quaternionf rotation(Eigen::AngleAxisf(-mt, Eigen::Vector3f::UnitZ()));    pcl::transformPointCloud(*frame, *cloud_transformed, translation, rotation);    // ICP to refine cloud alignment    if(false)//!cloud->empty())    {        //Use ICP to fix errors in transformation        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;        icp.setMaxCorrespondenceDistance(2);        icp.setMaximumIterations(30);        icp.setInputSource(cloud_transformed);        icp.setInputTarget(cloud);        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned (new pcl::PointCloud<pcl::PointXYZ>);        icp.align(*cloud_aligned);        if(icp.hasConverged())        {            //Add aligned cloud points to map cloud            (*cloud) += *cloud_aligned;        }        else        {            //Ignore alignment atempt            std::cout << "No convergence." << std::endl;            (*cloud) += *cloud_transformed;        }    }    else    {        //Add first cloud's points to map cloud        (*cloud) += *cloud_transformed;    }    //Run map cloud through a VoxelGrid to remove any duplicate points    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_removedDuplicates (new pcl::PointCloud<pcl::PointXYZ>);    pcl::VoxelGrid<pcl::PointXYZ> duplicateRemover;    duplicateRemover.setInputCloud(cloud);    float leafSize = ConfigManager::Instance().getValue("MapBuilder", "VoxelLeafSize", 0.25);    duplicateRemover.setLeafSize (leafSize, leafSize, leafSize);    duplicateRemover.filter(*cloud_removedDuplicates);    cloud.swap(cloud_removedDuplicates);    onNewMap(cloud);}

void IlinTest::test_translation(){    double radians = 0.5;    Angle angle;    translation(radians, &angle);    QCOMPARE(angle.degree, 28);    QCOMPARE(angle.min, 39);    QCOMPARE(angle.sec, 44);}

void Translation::run(vector<string> pArgParam,string input, string output){	unused(pArgParam);	ImagePNG* img = new ImagePNG();	img->load(input);	double start = Data::get_time();	cout << &img << endl;	translation(img,atoi(pArgParam[0].c_str()),atoi(pArgParam[1].c_str()), new Color(0,0,0,255));	if (Data::CHRONO)	cout << description<<" effectue en " << 1000*(Data::get_time() - start)<<" ms"<<endl;	img->save(output);}

void bimWorld::MovingCamera::translation(double startx, double starty, double endx, double endy){	auto manip = static_cast<PersonManipulator*>(m_host->_CameraManipulator()->getCameraManipulator());	if (!manip)		return;	manip->translation(startx, starty, endx, endy, m_host->_CameraManipulator()->getCamera());	m_host->_RenderingThreads()->updateSeveralTimes();}

Matrix4f Transform::getTransform() {	Matrix4f translation(this->translation, MATRIX4F_INIT_WITH_TRANSLATION);	Matrix4f rotation(this->rotation, MATRIX4F_INIT_WITH_ROTATION);	Matrix4f scale(this->scale, MATRIX4F_INIT_WITH_SCALE);//	Matrix4f cameraRotation(this->camera.getForward(), this->camera.getUp());//	Matrix4f cameraTranslation(-this->camera.getPos().getX(),-this->camera.getPos().getY(),-this->camera.getPos().getZ(),MATRIX4F_INIT_WITH_TRANSLATION);	return translation * rotation * scale;}

	Edge steerWithControl(const State &start, const std::vector<double> controls, double dt) const {		/* Be careful about the order these are being passed in */		fcl::Vec3f translation(controls[0], controls[1], controls[2]);		fcl::Quaternion3f rotation(controls[3], controls[4], controls[5], controls[6]);		rotation = math::normalize(rotation);		State end = doSteps(start, translation, rotation, dt);		return Edge(start, end, translation, rotation, dt);	}

void CameraObject::look_at (const Vector3 &to, const Vector3 &up){    Matrix3 m;	    Vector3 delta;        delta = translation() - to;        m = Matrix3::set_orientation(delta, up);    set_orientation(m);}

Matrix4d Sim3::matrix() const{  Matrix4d homogenious_matrix;  homogenious_matrix.setIdentity();  homogenious_matrix.block(0,0,3,3) = scale()*rotation_matrix();  homogenious_matrix.col(3).head(3) = translation();  homogenious_matrix(3,3) = 1;  return homogenious_matrix;}

void RenderLine2D(Shader *sh, Primitive prim, const float3 &v1, const float3 &v2, float thickness){    auto v = (v2 - v1) / 2;    auto len = length(v);    auto vnorm = v / len;    auto trans = translation(v1 + v) *                  rotationZ(vnorm.xy()) *                 float4x4(float4(len, thickness / 2, 1, 1));    RenderQuad(sh, prim, true, trans);}

void NGLScene::writeKey(){  std::cout << "KEY!" << std::endl;  ngl::Vec3 translation(0,0,0);  for(unsigned int i=0; i<m_geo.size(); i++ ){    m_rotHistory.push_back(m_geo[i]->m_rotation);    m_localRotHistory.push_back(m_geo[i]->m_localRotation);    if(i>0){      translation += m_geo[i-1]->getTopTranslation();    }    else{      translation.set(0,0,0);    }    m_transHistory.push_back(m_geo[i]->getTopTranslation());  }  if(m_rotHistory.size()/4 == Data::instance()->animLength){    //exit(writeOut());    m_buttons.clear();    std::cout << "adding" << std::endl;    ngl::Vec2 buttonPos;    ngl::Vec2 buttonSize;    float borderX = 50.0;    float borderY = 50.0;    borderX = borderX/m_width;    borderY = borderY/m_height;    float keyX = borderX * 2 - 1.0;    float keyY = borderY * 2 - 1.0;    float keyX2 = (keyX * -1);    buttonPos.set(keyX, keyY);    buttonSize.set(0.5, 0.55);    if(Data::instance()->mode == Data::TOUCHSMALL_R){      buttonPos.m_x *= -1;      buttonPos.m_x -= buttonSize.m_x;    }    addButton(buttonPos, buttonSize, Data::instance()->baseColor, Button::Action::FINISH);    updateButtonArrays();    m_finished = true;    m_timer2 = startTimer(1000/Data::instance()->fps);  }  else if(m_rotHistory.size()/4 > Data::instance()->animLength){    std::cerr << "ERROR, size of history too large" << std::endl;  }  m_frame++;  ngl::ShaderLib::instance()->use("progressShader");  ngl::ShaderLib::instance()->setRegisteredUniform("frame", m_frame);}

//--------------------------------------------------------------------------------------------------/// Pan the camera up/down and left/right/// /// /return  Returns true if input caused changes to the camera an false if no changes occurred//--------------------------------------------------------------------------------------------------bool ManipulatorTrackball::pan(int posX, int posY){    if (m_camera.isNull()) return false;    if (posX == m_lastPosX && posY == m_lastPosY) return false;    const double vpPixSizeX = m_camera->viewport()->width();    const double vpPixSizeY = m_camera->viewport()->height();    if (vpPixSizeX <= 0 || vpPixSizeY <= 0) return false;    // Normalized movement in screen plane     double tx = (posX - m_lastPosX)/vpPixSizeX;    double ty = (posY - m_lastPosY)/vpPixSizeY;    // Viewport size in world coordinates    const double aspect = m_camera->aspectRatio();    const double vpWorldSizeY = m_camera->frontPlaneFrustumHeight();    const double vpWorldSizeX = vpWorldSizeY*aspect;    const Vec3d camUp = m_camera->up();    const Vec3d camRight = m_camera->right();    Vec3d translation(0, 0, 0);    Camera::ProjectionType projType = m_camera->projection();    if (projType == Camera::PERSPECTIVE)    {        const Vec3d camPos = m_camera->position();        const Vec3d camDir = m_camera->direction();        const double nearPlane = m_camera->nearPlane();        // Compute distance from camera to rotation point projected onto camera forward direction        const Vec3d vDiff = m_rotationPoint - camPos;        const double camRotPointDist = Math::abs(camDir*vDiff);        Vec3d vX =  camRight*((tx*vpWorldSizeX)/nearPlane)*camRotPointDist;        Vec3d vY =  camUp*((ty*vpWorldSizeY)/nearPlane)*camRotPointDist;        translation = vX + vY;    }    else if (projType == Camera::ORTHO)    {        Vec3d vX =  camRight*tx*vpWorldSizeX;        Vec3d vY =  camUp*ty*vpWorldSizeY;        translation = vX + vY;    }    Mat4d viewMat = m_camera->viewMatrix();    viewMat.translatePostMultiply(translation);    m_camera->setViewMatrix(viewMat);    m_lastPosX = posX;    m_lastPosY = posY;    return true;}

void LifeCounter::Draw(mat4 view, mat4 projection) {	projection = ortho(0.0f, 20.0f * (scene->width / (float)scene->height), 0.0f, 20.0f, -20.0f, 20.0f);	for(int i=0; i<lives; i++) {		Content::shader(basic).Begin();			mat4 model = translation(position + vec3(i * icon.size.x, 0, 0))  * mat4_cast(orientation) * scale(size);			model = model;			glUniformMatrix4fv(Content::shader(basic)("inv_view"), 1, GL_FALSE, &(model[0][0]));			icon.Draw(model, projection);		Content::shader(basic).End();	}}

	STARTDECL(ph_render) ()	{		CheckPhysics();		auto oldobject2view = object2view;		auto oldcolor = curcolor;		for (b2Body *body = world->GetBodyList(); body; body = body->GetNext())		{			auto pos = body->GetPosition();			auto mat = translation(float3(pos.x, pos.y, 0)) * rotationZ(body->GetAngle());			object2view = oldobject2view * mat;			for (b2Fixture *fixture = body->GetFixtureList(); fixture; fixture = fixture->GetNext())			{				auto shapetype = fixture->GetType();				auto r = (Renderable *)fixture->GetUserData();				curcolor = r->color;				r->Set();				switch (shapetype)				{					case b2Shape::e_polygon:					{						auto polyshape = (b2PolygonShape *)fixture->GetShape();						RenderArray(PRIM_FAN, polyshape->m_count, "pn", sizeof(b2Vec2), polyshape->m_vertices, NULL, 							                                              sizeof(b2Vec2), polyshape->m_normals);						break;					}					case b2Shape::e_circle:					{						// FIXME: instead maybe cache a circle verts somewhere.. though should maxverts be changable?						const int maxverts = 20;						struct PhVert { float2 pos; float2 norm; } phverts[maxverts];						auto polyshape = (b2CircleShape *)fixture->GetShape();						float step = PI * 2 / maxverts;						for (int i = 0; i < maxverts; i++)						{							auto pos = float2(sinf(i * step + 1), cosf(i * step + 1));							phverts[i].pos = pos * polyshape->m_radius + *(float2 *)&polyshape->m_p;							phverts[i].norm = pos;						}						RenderArray(PRIM_FAN, maxverts, "pn", sizeof(PhVert), phverts, NULL);						break;					}					case b2Shape::e_edge:					case b2Shape::e_chain:          case b2Shape::e_typeCount:						assert(0);						break;				}			}		}		object2view = oldobject2view;		curcolor = oldcolor;		return Value();	}

void SimObject::setPosition(const Vector3d& pos){  Vector3d translation(pos);  translation -= position;  position = pos;  ObjectList::const_iterator listPos;  for(listPos = childNodes.begin(); listPos != childNodes.end(); ++listPos)  {    (*listPos)->translate(translation);  }}

void arc_ball_camera::update(float deltaTime){	glm::mat4 mat = glm::eulerAngleYX(_rotY, _rotX);	glm::vec4 translation(0.0f, 0.0f, _distance, 1.0f);	translation = mat * translation; 	_position = _target + glm::vec3(translation.x, translation.y, translation.z);	glm::vec4 tempUp(0.0f, 1.0f, 0.0f, 1.0f);	tempUp = mat * tempUp;	_up = glm::vec3(tempUp.x, tempUp.y, tempUp.z);	_view = glm::lookAt(_position, _target, _up);}

	Mat4 lookAt(const Vec3& eye, const Vec3& center, const Vec3& up)	{		Vec3 c_forward = (center - eye).normalize();		Vec3 c_right = c_forward.cross(up).normalize();		Vec3 c_up = c_right.cross(c_forward).normalize();		return Mat4(c_right.x, c_up.x, -c_forward.x, 0,			c_right.y, c_up.y, -c_forward.y, 0,			c_right.z, c_up.z, -c_forward.z, 0,			0, 0, 0, 1) * translation(-eye);	}

/** * @brief First Estimation of the fitted shape by scaling only * @param iShape -- input shape * @param rect   -- the rectangle to calculate the scalar * @return VO_Shape -- the scaled shape */VO_Shape VO_Fitting2DSM::VO_FirstEstimationByScaling(   const VO_Shape& iShape,        const cv::Rect& rect ){    VO_Shape res = iShape;    cv::Rect_<float> rect0 = iShape.GetShapeRect();    float fScaleX = (float)rect.width/rect0.width *0.80;    float fScaleY = (float)rect.height/rect0.height *0.80;    res.ScaleX(fScaleX);    res.ScaleY(fScaleY);    rect0 = iShape.GetShapeBoundRect();    cv::Mat_<float> translation = cv::Mat_<float>::zeros(2, 1);    float centerX = (float)rect.x + (float)rect.width/2.0f;    float centerY = (float)rect.y + (float)rect.height/2.0f;    float center0X = (float)rect0.x + (float)rect0.width/2.0f;    float center0Y = (float)rect0.x + (float)rect0.height/2.0f;    translation(0,0) = centerX - center0X;    translation(1,0) = centerY - center0Y;    res.Translate( translation );    return res;}

void ShapeColliderTests::sphereMissesCapsule() {    // non-overlapping sphere and capsule    float radiusA = 1.5f;    float radiusB = 2.3f;    float totalRadius = radiusA + radiusB;    float halfHeightB = 1.7f;    float axialOffset = totalRadius + 1.1f * halfHeightB;    float radialOffset = 1.2f * radiusA + 1.3f * radiusB;        SphereShape sphereA(radiusA);    CapsuleShape capsuleB(radiusB, halfHeightB);    // give the capsule some arbirary transform    float angle = 37.8f;    glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) );    glm::quat rotation = glm::angleAxis(angle, axis);    glm::vec3 translation(15.1f, -27.1f, -38.6f);    capsuleB.setRotation(rotation);    capsuleB.setPosition(translation);    CollisionList collisions(16);    // walk sphereA along the local yAxis next to, but not touching, capsuleB    glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f);    int numberOfSteps = 10;    float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1);    for (int i = 0; i < numberOfSteps; ++i) {        // translate sphereA into world-frame        glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis;        sphereA.setPosition(rotation * localPosition + translation);        // sphereA agains capsuleB        if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))        {            std::cout << __FILE__ << ":" << __LINE__                << " ERROR: sphere and capsule should NOT touch"                << std::endl;        }        // capsuleB against sphereA        if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))        {            std::cout << __FILE__ << ":" << __LINE__                << " ERROR: sphere and capsule should NOT touch"                << std::endl;        }    }    if (collisions.size() > 0) {        std::cout << __FILE__ << ":" << __LINE__            << " ERROR: expected empty collision list but size is " << collisions.size()            << std::endl;    }}

Matrix2d Matrix2d::transformWith2P(const Point2d& from1, const Point2d& from2,                                   const Point2d& to1, const Point2d& to2){    if (from1 == from2 || to1 == to2        || from1.isDegenerate() || from2.isDegenerate() || to1.isDegenerate() || to2.isDegenerate()) {        return Matrix2d::kIdentity();    }    return (translation(to1 - from1)            * scaling(to2.distanceTo(to1) / from2.distanceTo(from1), to1)            * rotation((to2 - to1).angle2() - (from2 - from1).angle2(), to1));}

///fn void ExtrinsicParam::changePanTilt(double pan, double tilt);////brief This function computes the new rotation matrix and the new translation vector of the extrinsic parameters when the camera has changed its position.////param pan Value of the new camera panoramique.////param tilt Value of the new camera tilt.void ExtrinsicParam::changePanTilt(double pan, double tilt){  // Compute the rotation matrices with the new values of pan and tilt  Eigen::Matrix3d Rx, Ry;  Rx.setZero();  Ry.setZero();  Rx(0,0) = 1;  Rx(1,1) = cos((-(tilt-this->tilt))*PI/180.);  Rx(1,2) = -sin((-(tilt-this->tilt))*PI/180.);  Rx(2,1) = sin((-(tilt-this->tilt))*PI/180.);  Rx(2,2) = cos((-(tilt-this->tilt))*PI/180.);  Ry(0,0) = cos((-(pan-this->pan))*PI/180.);  Ry(0,2) = sin((-(pan-this->pan))*PI/180.);  Ry(1,1) = 1;  Ry(2,0) = -sin((-(pan-this->pan))*PI/180.);  Ry(2,2) = cos((-(pan-this->pan))*PI/180.);  Eigen::Vector3d Tx, Ty;  Tx.setZero();  Ty.setZero();  Tx(0) = 2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos(0.5*(this->pan-pan)*PI/180.);  Tx(2) = -2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos((90.-0.5*(this->pan-pan))*PI/180.);  Ty(1) = 2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos(0.5*(this->tilt-tilt)*PI/180.);  Ty(2) = -2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos((90.-0.5*(this->tilt-tilt))*PI/180.);  // Compute the new values of the extrinsic parameters  Eigen::Matrix4d Rx1, Ry1, Rt;  Rt << initial_rotation, initial_translation,    0,0,0,1;  Rx1 << Rx, Tx,    0,0,0,1;  Ry1 << Ry, Ty,    0,0,0,1;  Rt.noalias() = Rx1*Ry1*Rt;  rotation(0,0) = Rt(0,0);rotation(0,1) = Rt(0,1);rotation(0,2) = Rt(0,2);  rotation(1,0) = Rt(1,0);rotation(1,1) = Rt(1,1);rotation(1,2) = Rt(1,2);  rotation(2,0) = Rt(2,0);rotation(2,1) = Rt(2,1);rotation(2,2) = Rt(2,2);  translation(0) = Rt(0,3);  translation(1) = Rt(1,3);  translation(2) = Rt(2,3);}

	Matrix44f Camera::GetInverse() const	{		Matrix44f tempMatrix = myTransformation;		Vector4f translation(tempMatrix.GetPosition());		tempMatrix.SetPosition(Vector3f(0.0f, 0.0f, 0.0f));		translation *= -1;		translation.w = 1;		tempMatrix.Transpose();		translation = translation * tempMatrix;		tempMatrix.SetPosition(Vector3f(translation.x, translation.y, translation.z));		return tempMatrix;	}

void SceneGraph::link(TransformInstance child, TransformInstance parent){	unlink(child);	if (!is_valid(_data.first_child[parent.i]))	{		_data.first_child[parent.i] = child;		_data.parent[child.i] = parent;	}	else	{		TransformInstance prev = { UINT32_MAX };		TransformInstance node = _data.first_child[parent.i];		while (is_valid(node))		{			prev = node;			node = _data.next_sibling[node.i];		}		_data.next_sibling[prev.i] = child;		_data.first_child[child.i].i = UINT32_MAX;		_data.next_sibling[child.i].i = UINT32_MAX;		_data.prev_sibling[child.i] = prev;	}	Matrix4x4 parent_tr = _data.world[parent.i];	Matrix4x4 child_tr = _data.world[child.i];	const Vector3 cs = scale(child_tr);	Vector3 px = x(parent_tr);	Vector3 py = y(parent_tr);	Vector3 pz = z(parent_tr);	Vector3 cx = x(child_tr);	Vector3 cy = y(child_tr);	Vector3 cz = z(child_tr);	set_x(parent_tr, normalize(px));	set_y(parent_tr, normalize(py));	set_z(parent_tr, normalize(pz));	set_x(child_tr, normalize(cx));	set_y(child_tr, normalize(cy));	set_z(child_tr, normalize(cz));	const Matrix4x4 rel_tr = child_tr * get_inverted(parent_tr);	_data.local[child.i].position = translation(rel_tr);	_data.local[child.i].rotation = to_matrix3x3(rel_tr);	_data.local[child.i].scale = cs;	_data.parent[child.i] = parent;	transform(parent_tr, child);}

const CU::Matrix33<float> Camera2D::GetInverse() const{	CU::Vector3<float> translation(-myOrientation.myMatrix[6], -myOrientation.myMatrix[7], 1);	CU::Matrix33<float> inverse(myOrientation);	inverse.myMatrix[6] = 0;	inverse.myMatrix[7] = 0;	inverse = CU::Transpose(inverse);	translation *= inverse;	inverse.myMatrix[6] = translation.x;	inverse.myMatrix[7] = translation.y;	return inverse;}

// Normalize a blob using blob transformations. Less accurate, but// more accurately copies the old way.void DENORM::LocalNormBlob(TBLOB* blob) const {  TBOX blob_box = blob->bounding_box();  ICOORD translation(-IntCastRounded(x_origin_), -IntCastRounded(y_origin_));  blob->Move(translation);  if (y_scale_ != 1.0f)    blob->Scale(y_scale_);  if (rotation_ != NULL)    blob->Rotate(*rotation_);  translation.set_x(IntCastRounded(final_xshift_));  translation.set_y(IntCastRounded(final_yshift_));  blob->Move(translation);}

void callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msf)//,//                    const geometry_msgs::PoseStamped::ConstPtr &pose_sfm){  // Extract translation and rotation  Eigen::Vector3f translation(pose_msf->pose.pose.position.x,                               pose_msf->pose.pose.position.y,                               pose_msf->pose.pose.position.z);  Eigen::Quaternionf q(pose_msf->pose.pose.orientation.w,                        pose_msf->pose.pose.orientation.x,                        pose_msf->pose.pose.orientation.y,                        pose_msf->pose.pose.orientation.z);    img.copyTo(img_draw);  draw(q, translation, cv::Scalar(255, 0, 0), tail);      draw(q_sfm, t_sfm, cv::Scalar(200, 200, 200), tail_sfm, false);/*  int c = img_cam.cols;  int r = img_cam.rows;  int cd = img_draw.cols;  int rd = img_draw.rows;  cv::Mat img_cam_r;  cv::resize(img_cam, img_cam_r, cv::Size(rd * c / r, rd));//  std::cout << img_cam_r.size() << std::endl;//  std::cout << img_draw.size() << std::endl;  cv::Mat img_sbs(img_draw.rows, img_draw.cols + img_cam_r.cols, CV_32FC3);//  img_draw.convertTo(img_draw, CV_32FC3);//  img_cam_r.convertTo(img_cam_r, CV_32FC3);  for (size_t c = 0; c < img_draw.cols; ++c)  {    img_sbs.col(c) = img_draw.col(c);  }  for (size_t c = 0; c < img_cam_r.cols; ++c)  {    img_sbs.col(c + img_draw.cols) = img_cam_r.col(c);  }*/  cv::resize(img_draw, img_draw, cv::Size(), 0.7, 0.7); // cv::namedWindow("2d-1");  cv::namedWindow("2d-2"); // cv::imshow("2d-1", img_cam_r);  cv::imshow("2d-2", img_draw);  cv::waitKey(1);}