I have 2 Points in 3D P1(x1,y1,z1) & P2(x2,y2,z2) with Origin at Origin(0,0,0).
Now I want to shift the Origin to P1(x1,y1,z1) with new Rotation vector R.
How to represent the Point P2(x2,y2,z2) with respect to the new Origin?
How to compute the Affine Transformation Matrix of Point P1 & Origin, so that the same Point P2 Can be represented with respect to P1 as Origin?
To Explain My problem I’ve added a simple 3D Simulation to verify my Result!
When there is No Rotation involved the Points are Exactly the same as shown in this Image. You can Notice that Both Red and Yellow Lines are overlapping in the Image.
But When there is some Rotation is involved in the New Origin, Then the Points are not matching!
Here is the code to reproduce the same results!
#include <iostream>
#include <fstream>
#include <string>
#include <Windows.h>
#include <gl/GL.h>
#include <gl/GLU.h>
#pragma comment( lib, "opengl32.lib" )
#pragma comment( lib, "glu32.lib" )
#include <opencv2/opencv.hpp>
#include <opencv2/plot.hpp>
#include <opencv2/viz.hpp>
using namespace std;
using namespace cv;
int main(int argc)
{
const int
IMAGE_WIDTH = 1280,
IMAGE_HEIGHT = 720;
const double
fx = IMAGE_WIDTH,
fy = IMAGE_HEIGHT,
cx = IMAGE_WIDTH / 2.0,
cy = IMAGE_HEIGHT / 2.0;
double camMatarray[9] = {
fx, 0., cx,
0., fy, cy,
0., 0., 1.
};
Matx33d CameraMatrix = Matx33d(
fx, 0, cx,
0, fy, cy,
0, 0, 1);
Point3d ptv_VehicleOrigin(0, 0, 0); //Origin
Point3d ptV_Ball3D(4238, 0, 324.00);//Point P1
Point3d ptV_Camera3D(3778, 53, 650);//Point P2
Point3d ptV_Camera3D_Orientation(5, 197, 0); //Not -Working Case
//Point3d ptV_Camera3D_Orientation(0, 0, 0); //Working Case
/// Create 3D windows
Affine3d cam_pose = Affine3d(Mat::eye(3, 3, CV_64F), ptV_Camera3D);
viz::Viz3d Window_3D("World 3D Frame");
Window_3D.setWindowSize(Size(IMAGE_WIDTH, IMAGE_HEIGHT));
Window_3D.setBackgroundColor(); // black by default
Window_3D.setViewerPose(cam_pose);
while (!Window_3D.wasStopped())
{
Affine3d AffineTransform = Affine3d(Mat::eye(3, 3, CV_64F), ptV_Camera3D);
//Apply Translation
Point3d ptC_Ball3D = ptV_Ball3D - ptV_Camera3D;
cout << "Ball 2 Camera Translation: " << ptC_Ball3D << "\n";
//Apply Rotation
Vec3f R = Vec3f((float)ptV_Camera3D_Orientation.x, (float)ptV_Camera3D_Orientation.y, (float)ptV_Camera3D_Orientation.z);
AffineTransform.rotate(R);
Affine3d Rotation = Affine3d(eulerAnglesToRotationMatrix(R));
ptC_Ball3D = Rotation * ptC_Ball3D;
//Apply Rotation
cout << "Ball 2 Camera Rotation: " << ptC_Ball3D << "\n";
//Window_3D.setWidgetPose
viz::WText3D TextBall(cv::format("Ball [%.0f,%.0f,%.0f]", ptV_Ball3D.x, ptV_Ball3D.y, ptV_Ball3D.z), ptV_Ball3D, 15.0);
Window_3D.showWidget("Ball Text", TextBall);
viz::WText3D TextCamera(cv::format("Camera [%.0f,%.0f,%.0f]", ptV_Camera3D.x, ptV_Camera3D.y, ptV_Camera3D.z), ptV_Camera3D, 15.0);
Window_3D.showWidget("Camera Text", TextCamera);
viz::WText3D TextBall2(cv::format("Ball2 [%.0f,%.0f,%.0f]", ptC_Ball3D.x, ptC_Ball3D.y, ptC_Ball3D.z), ptC_Ball3D, 15.0, false);
Window_3D.showWidget("Camera Text1", TextBall2, AffineTransform);
viz::WLine lineOrigin2Camera(ptv_VehicleOrigin, ptV_Camera3D, cv::viz::Color::blue());
lineOrigin2Camera.setRenderingProperty(viz::LINE_WIDTH, 2.0);
Window_3D.showWidget("Line Origin2Camera", lineOrigin2Camera);
viz::WLine lineCamera2Ball(ptV_Camera3D, ptV_Ball3D, cv::viz::Color::yellow());
lineCamera2Ball.setRenderingProperty(viz::LINE_WIDTH, 2.0);
Window_3D.showWidget("Line Camera2Ball", lineCamera2Ball);
viz::WLine lineOrigin2Ball(ptv_VehicleOrigin, ptV_Ball3D, cv::viz::Color::green());
lineOrigin2Ball.setRenderingProperty(viz::LINE_WIDTH, 2.0);
Window_3D.showWidget("Line Origin2Ball", lineOrigin2Ball);
viz::WLine lineCamera2Ball_1(ptv_VehicleOrigin, ptC_Ball3D, cv::viz::Color::red());
lineCamera2Ball_1.setRenderingProperty(viz::LINE_WIDTH, 2.0);
Window_3D.showWidget("Line Camera2Ball1", lineCamera2Ball_1, AffineTransform);
// Create a sphere widget
viz::WSphere swBall(Point3d(ptV_Ball3D), 10.01f, 10, cv::viz::Color::blue());
// Cast sphere widget to cloud widget
viz::WCloud cwBall = swBall.cast<viz::WCloud>();
/// Modify it, and it will be modified in the window.
cwBall.setColor(viz::Color::red());
// Create a sphere widget
viz::WSphere swCamera(ptV_Camera3D, 10.01f, 0.25, cv::viz::Color::yellow());
// Cast sphere widget to cloud widget
viz::WCloud cwCamera = swCamera.cast<viz::WCloud>();
/// Modify it, and it will be modified in the window.
cwCamera.setColor(viz::Color::red());
// Display it in a window
Window_3D.showWidget("SphereBall", cwBall);
cam_pose = Affine3d(ptV_Camera3D_Orientation, ptV_Camera3D);
Affine3d vehicle_pose = Affine3d(Mat::eye(3, 3, CV_64F), ptv_VehicleOrigin);
viz::WCameraPosition cpw(500); // Coordinate axes
viz::WCameraPosition cpw_frustum(CameraMatrix, 150, viz::Color::yellow()); // Camera frustum
viz::WCameraPosition VehicleAxis(500); // Coordinate axes
viz::WCameraPosition VehicleAxis_frustum(Matx33d::eye(), 150, viz::Color::yellow()); // Camera frustum
Window_3D.showWidget("SphereCamera", cwCamera);
Window_3D.showWidget("CPW", cpw, cam_pose);
Window_3D.showWidget("VehicleAxis", VehicleAxis, vehicle_pose);
Window_3D.spinOnce(100, true);
Window_3D.removeAllWidgets();
}
Window_3D.close();
waitKey();
return 0;
}