| Line 183... |
Line 183... |
| 183 |
QSettings settings;
|
183 |
QSettings settings;
|
| 184 |
QVariant calibrationVariant = settings.value("calibration/parameters");
|
184 |
QVariant calibrationVariant = settings.value("calibration/parameters");
|
| 185 |
SMCalibrationParameters calibration = calibrationVariant.value<SMCalibrationParameters>();
|
185 |
SMCalibrationParameters calibration = calibrationVariant.value<SMCalibrationParameters>();
|
| 186 |
|
186 |
|
| 187 |
// camera 0 coordinate system
|
187 |
// camera 0 coordinate system
|
| 188 |
visualizer->removeCoordinateSystem("camera0", 0);
|
188 |
//visualizer->removeCoordinateSystem("camera0", 0);
|
| 189 |
visualizer->addCoordinateSystem(100, "camera0", 0);
|
189 |
visualizer->addCoordinateSystem(100, "camera0", 0);
|
| 190 |
|
190 |
|
| 191 |
// camera 1 coordinate system
|
191 |
// camera 1 coordinate system
|
| 192 |
cv::Mat Transform1CV(3, 4, CV_32F);
|
192 |
cv::Mat Transform1CV(3, 4, CV_32F);
|
| 193 |
cv::Mat(calibration.R1).copyTo(Transform1CV.colRange(0, 3));
|
193 |
cv::Mat(calibration.R1).copyTo(Transform1CV.colRange(0, 3));
|
| 194 |
cv::Mat(calibration.T1).copyTo(Transform1CV.col(3));
|
194 |
cv::Mat(calibration.T1).copyTo(Transform1CV.col(3));
|
| 195 |
Eigen::Affine3f Transform1;
|
195 |
Eigen::Affine3f Transform1;
|
| 196 |
cv::cv2eigen(Transform1CV, Transform1.matrix());
|
196 |
cv::cv2eigen(Transform1CV, Transform1.matrix());
|
| 197 |
|
197 |
|
| - |
|
198 |
//visualizer->removeCoordinateSystem("camera1", 0);
|
| 198 |
visualizer->addCoordinateSystem(100, Transform1.inverse(), "camera1", 0);
|
199 |
visualizer->addCoordinateSystem(100, Transform1.inverse(), "camera1", 0);
|
| 199 |
|
200 |
|
| 200 |
// rotation axis coordinate system
|
201 |
// // rotation axis coordinate system
|
| 201 |
// cv::Mat TransformRCV(3, 4, CV_32F);
|
202 |
// cv::Mat TransformRCV(3, 4, CV_32F);
|
| 202 |
// cv::Mat(calibration.Rr).copyTo(TransformRCV.colRange(0, 3));
|
203 |
// cv::Mat(calibration.Rr).copyTo(TransformRCV.colRange(0, 3));
|
| 203 |
// cv::Mat(calibration.Tr).copyTo(TransformRCV.col(3));
|
204 |
// cv::Mat(calibration.Tr).copyTo(TransformRCV.col(3));
|
| 204 |
// Eigen::Affine3f TransformR;
|
205 |
// Eigen::Affine3f TransformR;
|
| 205 |
// cv::cv2eigen(TransformRCV, TransformR.matrix());
|
- |
|
| 206 |
|
206 |
|
| - |
|
207 |
// cv::cv2eigen(TransformRCV, TransformR.matrix());
|
| 207 |
// visualizer->addCoordinateSystem(100, TransformR);
|
208 |
// visualizer->removeCoordinateSystem("camera1", 0);
|
| - |
|
209 |
// visualizer->addCoordinateSystem(100, TransformR.inverse(), "rotation", 0);
|
| 208 |
|
210 |
|
| 209 |
// rotation axis pointing (0,1,0) in rotation stage frame
|
211 |
// rotation axis pointing (0,1,0) in rotation stage frame
|
| 210 |
cv::Vec3f O(0.0, 00.0, 0.0);
|
212 |
cv::Vec3f O(0.0, 0.0, 0.0);
|
| 211 |
cv::Vec3f v(0.0, 600.0, 0.0);
|
213 |
cv::Vec3f v(0.0, 600.0, 0.0);
|
| 212 |
|
214 |
|
| 213 |
// determine coefficients in camera 0 frame
|
215 |
// determine coefficients in camera 0 frame
|
| 214 |
O = calibration.Rr.t()*O + -calibration.Rr.t()*calibration.Tr;
|
216 |
O = calibration.Rr.t()*O + -calibration.Rr.t()*calibration.Tr;
|
| 215 |
v = calibration.Rr.t()*v + -calibration.Rr.t()*calibration.Tr - O;
|
217 |
v = calibration.Rr.t()*v + -calibration.Rr.t()*calibration.Tr - O;
|