//------------------------------------------------------------------------------
// Rvector GetRvectorParameter(const Integer id, const Integer index) const
//------------------------------------------------------------------------------
Rvector Array::GetRvectorParameter(const Integer id, const Integer index) const
{
switch (id)
{
case ROW_VALUE:
{
Rvector rvec(mNumCols);
for (int i=0; i<mNumCols; i++)
rvec.SetElement(i, mRmatValue.GetElement(index, i));
return rvec;
}
case COL_VALUE:
{
Rvector rvec(mNumRows);
for (int i=0; i<mNumRows; i++)
rvec.SetElement(i, mRmatValue.GetElement(i, index));
return rvec;
}
default:
throw ParameterException
("Array::GetRvectorParameter() Unknown Parameter Name" + PARAMETER_TEXT[id]);
//return Parameter::GetRvectorParameter(id, index);
}
}
void
MainWindow::processInput(CGAL::Object o)
{
std::list<Point_2> input;
if(CGAL::assign(input, o)){
Point_2 p = input.front();
mc.insert(p);
me.insert(p);
points.push_back(p);
convex_hull.push_back(p);
Polygon_2 tmp;
CGAL::convex_hull_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(tmp));
convex_hull = tmp;
min_rectangle.clear();
CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));
min_parallelogram.clear();
CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));
std::vector<Point_2> center;
double radius;
if (points.size()>=P){
CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
Vector_2 rvec(radius, radius);
for(std::size_t i=0; i < center.size(); i++){
p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
}
}
}
emit(changed());
}
void GLRender::computeModelViewMatrix()
{
cv::Matx31f rvec(m_rotation);
cv::Matx33f rmat;
cv::Rodrigues(rvec, rmat);
rmat = rmat.inv();
m_modelViewMatrix[0] = rmat.val[0];
m_modelViewMatrix[1] = rmat.val[3];
m_modelViewMatrix[2] = rmat.val[6];
m_modelViewMatrix[3] = 0.0f;
m_modelViewMatrix[4] = rmat.val[1];
m_modelViewMatrix[5] = rmat.val[4];
m_modelViewMatrix[6] = rmat.val[7];
m_modelViewMatrix[7] = 0.0f;
m_modelViewMatrix[8] = rmat.val[2];
m_modelViewMatrix[9] = rmat.val[5];
m_modelViewMatrix[10] = rmat.val[8];
m_modelViewMatrix[11] = 0.0f;
cv::Matx31f tvec(m_translation);
tvec = rmat*tvec;
m_modelViewMatrix[12] = -tvec.val[0];
m_modelViewMatrix[13] = -tvec.val[1];
m_modelViewMatrix[14] = -tvec.val[2];
m_modelViewMatrix[15] = 1.0f;
}
/*!
\brief Decomposes the Matrix into three Euler angles.
\param[out] hy Heading in radians.
\param[out] px Pitch in radians.
\param[out] rz Roll in radians.
*/
void
dmz::Matrix::to_euler_angles (Float64 &hy, Float64 &px, Float64 &rz) const {
Vector hvec (Forward), pvec (Forward), rvec (Up);
transform_vector(hvec);
Matrix cmat (*this), hmat, pmat, rmat;
hvec.set_y (0.0);
if (hvec.is_zero ()) { hy = 0.0; }
else {
hvec.normalize_in_place ();
hy = Forward.get_signed_angle (hvec);
hmat.from_axis_and_angle (Up, hy);
hmat.transpose_in_place ();
cmat = hmat * cmat;
}
cmat.transform_vector (pvec);
if (is_zero64 (pvec.get_y ())) { px = 0.0; }
else {
px = Forward.get_signed_angle (pvec);
pmat.from_axis_and_angle (Right, px);
pmat.transpose_in_place ();
cmat = pmat * cmat;
}
cmat.transform_vector (rvec);
if (is_zero64 (rvec.get_x ())) { rz = 0.0; }
else { rz = Up.get_signed_angle (rvec); }
}
rvec linspace( double from, double to, int N )
{
// if( N < 0 ) {
// assert( mod(to-from,1) == 0 );
// N = abs(to-from) + 1;
// }
// Case N == 0
if( N == 0 )
return rvec();
// Case from == to
if( from == to )
return from * ones(N);
// Case N == 1 and from ~= to
if( N == 1 )
error("linspace() - At least two points need to be generated");
// Normal case: N > 1 and from ~= to
double skip = (to-from) / (N-1);
rvec x(N);
for( int n = 0; n < N; n++ )
x(n) = from + n*skip;
return x;
}
Vector Vector::operator+() const
{
Vector rvec(n); // Make a vector of size n to return
for(int i = 0; i < n; i++) { //copy in the values
rvec[i] = v[i];
}
return rvec;
}
Vector Vector::operator-() const
{
Vector rvec(n);
for (int i = 0; i<n; i++) {
rvec[i] = -v[i];
}
return rvec;
}
real AbstractTry::median(const rvec &list)
{
if (list.size() == 0)
return 0;
rvec sort = rvec(list);
qSort(sort.begin(), sort.end());
return sort[sort.size()/2];
}
void LeapToCameraCalibrator::correctCameraPNP (ofxCv::Calibration & myCalibration){
vector<cv::Point2f> imagePoints;
vector<cv::Point3f> worldPoints;
if( hasFingerCalibPoints ){
for (int i=0; i<calibVectorImage.size(); i++){
imagePoints.push_back(ofxCvMin::toCv(calibVectorImage[i]));
worldPoints.push_back(ofxCvMin::toCv(calibVectorWorld[i]));
}
}
else{
cout << "not enough control points" << endl;
return;
}
cout << "myCalibration.getDistCoeffs() " << myCalibration.getDistCoeffs() << endl;
cout << "myCalibration.getUndistortedIntrinsics().getCameraMatrix() " << myCalibration.getUndistortedIntrinsics().getCameraMatrix() << endl;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
cv::solvePnP(worldPoints,imagePoints,
myCalibration.getUndistortedIntrinsics().getCameraMatrix(), myCalibration.getDistCoeffs(),
rvec, tvec, false );
// solvePnP( InputArray objectPoints, InputArray imagePoints,
// InputArray cameraMatrix, InputArray distCoeffs,
// OutputArray rvec, OutputArray tvec,
// bool useExtrinsicGuess=false );
calibrated = true;
setExtrinsics(rvec, tvec);
setIntrinsics(myCalibration.getUndistortedIntrinsics().getCameraMatrix());
this->camera = myCalibration.getUndistortedIntrinsics().getCameraMatrix();
cv::Mat distortionCoefficients = cv::Mat::zeros(5, 1, CV_64F);
this->distortion = distortionCoefficients;
this->rotation = rvec;
this->translation = tvec;
cout << "camera:\n";
cout << this->camera << "\n";
cout << "RVEC:\n";
cout << rvec << "\n";
cout << "TVEC:\n";
cout << tvec << "\n";
cout << "--------\n";
}
void ndarray_fromstring(const unsigned char *input, int size, std::vector<double> *vec, std::vector<int> *shape) {
// TODO: Look at optimizing this
msgpack::unpacked msg;
msgpack::sbuffer sbuf;
sbuf.write((const char *)input, size);
msgpack::unpack(&msg, sbuf.data(), sbuf.size());
msgpack::type::tuple<std::vector<double>, std::vector<int> > rvec(msg.get());
msgpack::type::tuple_element<msgpack::type::tuple<std::vector<double>, std::vector<int> >, 0> rvec0(rvec);
*vec = rvec0.get();
msgpack::type::tuple_element<msgpack::type::tuple<std::vector<double>, std::vector<int> >, 1> rvec1(rvec);
*shape = rvec1.get();
}
int main( int argc, char* argv[])
{
// Read points
std::vector<cv::Point2f> imagePoints = Generate2DPoints();
std::vector<cv::Point3f> objectPoints = Generate3DPoints();
std::cout << "There are " << imagePoints.size() << " imagePoints and " << objectPoints.size() << " objectPoints." << std::endl;
//camera parameters
double fx = 525.0; //focal length x
double fy = 525.0; //focal le
double cx = 319.5; //optical centre x
double cy = 239.5; //optical centre y
cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
cameraMatrix.at<double>(0,0)=fx;
cameraMatrix.at<double>(1,1)=fy;
cameraMatrix.at<double>(2,2)=1;
cameraMatrix.at<double>(0,2)=cx;
cameraMatrix.at<double>(1,2)=cy;
cameraMatrix.at<double>(0,1)=0;
cameraMatrix.at<double>(1,0)=0;
cameraMatrix.at<double>(2,0)=0;
cameraMatrix.at<double>(2,1)=0;
std::cout << "Initial cameraMatrix: " << cameraMatrix << std::endl;
cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "rvec: " << rvec << std::endl;
std::cout << "tvec: " << tvec << std::endl;
std::vector<cv::Point2f> projectedPoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
}
return 0;
}
void image_detections_fromstring(const unsigned char *input, int size, std::string *image_str, std::vector<double> *vec, std::vector<int> *shape) {
msgpack::unpacked msg;
msgpack::sbuffer sbuf;
sbuf.write((const char *)input, size);
msgpack::unpack(&msg, sbuf.data(), sbuf.size());
msgpack::type::tuple<std::string, std::vector<double>, std::vector<int> > rvec(msg.get());
msgpack::type::tuple_element<msgpack::type::tuple<std::string, std::vector<double>, std::vector<int> >, 0> rvec0(rvec);
msgpack::type::tuple_element<msgpack::type::tuple<std::string, std::vector<double>, std::vector<int> >, 1> rvec1(rvec);
msgpack::type::tuple_element<msgpack::type::tuple<std::string, std::vector<double>, std::vector<int> >, 2> rvec2(rvec);
*image_str = rvec0.get();
*vec = rvec1.get();
*shape = rvec2.get();
}
Vector Vector::operator-(const Vector& u) const
{
int size = u.size();
size = ( n < size ? n : size );
// If different, warn the user, but carry on
if(size != n) {
throw(Error("WARNING", "Vectors are different sizes."));
}
Vector rvec(size);
for (int i = 0; i < size; i++) { // Subtract
rvec[i] = v[i]-u.v[i]; // This is a left-to-right operator
}
return rvec;
}
Vector Vector::operator+(const Vector& u) const
{
int size = u.size();
size = ( n < size ? n : size ); // Set size to the smaller of the vector sizes
// If different, warn the user, but carry on
if(size != n) {
throw( Error("WARNING", "Vectors are different sizes.") );
}
Vector rvec(size); // If one vector is bigger, excess is lost
for (int i = 0; i < size; i++) { // Do the addition
rvec[i] = v[i]+u.v[i];
}
return rvec;
}
void WeaponItemSubSystem::SetProjectilePosition(Actor& projectile, Actor& actor)
{
Opt<IPositionComponent> projPositionC = projectile.Get<IPositionComponent>();
Opt<IPositionComponent> actorPositionC = actor.Get<IPositionComponent>();
glm::vec2 rvec(0.0, 0.0);
Opt<IInventoryComponent> inventoryC = actor.Get<IInventoryComponent>();
if (inventoryC.IsValid())
{
Opt<Weapon> weapon = inventoryC->GetSelectedWeapon();
if (weapon.IsValid())
{
rvec.x += weapon->GetPositionX();
rvec.y -= weapon->GetPositionY();
}
}
rvec=glm::rotate(rvec, float(actorPositionC->GetOrientation()));
projPositionC->SetX( projPositionC->GetX() + actorPositionC->GetX() + rvec.x );
projPositionC->SetY( projPositionC->GetY() + actorPositionC->GetY() + rvec.y );
}
cv::Mat OpenCVCameraEstimation::estimate(std::vector<imageio::ModelLandmark> imagePoints, cv::Mat intrinsicCameraMatrix, std::vector<int> vertexIds /*= std::vector<int>()*/)
{
if (imagePoints.size() < 3) {
Loggers->getLogger("morphablemodel").error("CameraEstimation: Number of points given is smaller than 3.");
throw std::runtime_error("CameraEstimation: Number of points given is smaller than 3.");
}
// Todo: Currently, the optional vertexIds is not used
vector<Point2f> points2d;
vector<Point3f> points3d;
for (const auto& landmark : imagePoints) {
points2d.emplace_back(landmark.getPoint2D());
points3d.emplace_back(morphableModel.getShapeModel().getMeanAtPoint(landmark.getName()));
}
//Estimate the pose
Mat rvec(3, 1, CV_64FC1);
Mat tvec(3, 1, CV_64FC1);
if (points2d.size() == 3) {
cv::solvePnP(points3d, points2d, intrinsicCameraMatrix, vector<float>(), rvec, tvec, false, CV_ITERATIVE); // CV_ITERATIVE (3pts) | CV_P3P (4pts) | CV_EPNP (4pts)
} else {
cv::solvePnP(points3d, points2d, intrinsicCameraMatrix, vector<float>(), rvec, tvec, false, CV_EPNP); // CV_ITERATIVE (3pts) | CV_P3P (4pts) | CV_EPNP (4pts)
// Alternative, more outlier-resistant:
// cv::solvePnPRansac(modelPoints, imagePoints, camMatrix, distortion, rvec, tvec, false); // min 4 points
// has an optional argument 'inliers' - might be useful
}
// Convert rvec/tvec to matrices, etc... return 4x4 extrinsic camera matrix
Mat rotation_matrix(3, 3, CV_64FC1);
cv::Rodrigues(rvec, rotation_matrix);
rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
Mat translation_vector = tvec;
translation_vector.convertTo(translation_vector, CV_32FC1);
Mat extrinsicCameraMatrix = Mat::zeros(4, 4, CV_32FC1);
Mat extrRot = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(0, 3));
rotation_matrix.copyTo(extrRot);
Mat extrTrans = extrinsicCameraMatrix(cv::Range(0, 3), cv::Range(3, 4));
translation_vector.copyTo(extrTrans);
extrinsicCameraMatrix.at<float>(3, 3) = 1; // maybe set (3, 2) = 1 here instead so that the renderer can do divByW as well? (see Todo in libRender)
return extrinsicCameraMatrix;
}
cv::Mat GoodFrame::getRT(std::vector<cv::Point3f> &modelPoints_min)
{
cv::Mat img = this->getCapturesImage();
std::vector<cv::Point2f> imagePoints;
for (size_t i = 0; i < 68; ++i)
{
imagePoints.push_back(cv::Point2f((float)(this->getDetected_landmarks().at<double>(i)),
(float)(this->getDetected_landmarks().at<double>(i+68))));
}
/////
int max_d = MAX(img.rows,img.cols);
cv::Mat camMatrix = (Mat_<double>(3,3) << max_d, 0, img.cols/2.0,
0, max_d, img.rows/2.0,
0, 0, 1.0);
cv::Mat ip(imagePoints);
cv::Mat op(modelPoints_min);
std::vector<double> rv(3), tv(3);
cv::Mat rvec(rv),tvec(tv);
double _dc[] = {0,0,0,0};
// std::cout << ip << std::endl << std::endl;
// std::cout << op << std::endl << std::endl;
// std::cout << camMatrix << std::endl << std::endl;
solvePnP(op, ip, camMatrix, cv::Mat(1,4,CV_64FC1,_dc), rvec, tvec, false, CV_EPNP);
double rot[9] = {0};
cv::Mat rotM(3, 3, CV_64FC1, rot);
cv::Rodrigues(rvec, rotM);
double* _r = rotM.ptr<double>();
// printf("rotation mat: \n %.3f %.3f %.3f\n%.3f %.3f %.3f\n%.3f %.3f %.3f\n",_r[0],_r[1],_r[2],_r[3],_r[4],_r[5],_r[6],_r[7],_r[8]);
// printf("trans vec: \n %.3f %.3f %.3f\n",tv[0],tv[1],tv[2]);
cv::Mat _pm(3, 4, CV_64FC1);
_pm.at<double>(0,0) = _r[0]; _pm.at<double>(0,1) = _r[1]; _pm.at<double>(0,2) = _r[2]; _pm.at<double>(0,3) = tv[0];
_pm.at<double>(1,0) = _r[3]; _pm.at<double>(1,1) = _r[4]; _pm.at<double>(1,2) = _r[5]; _pm.at<double>(1,3) = tv[1];
_pm.at<double>(2,0) = _r[6]; _pm.at<double>(2,1) = _r[7]; _pm.at<double>(2,2) = _r[8]; _pm.at<double>(2,3) = tv[2];
return _pm;
}
void
MainWindow::update_from_points()
{
convex_hull.clear();
CGAL::convex_hull_2(points.begin(), points.end(), std::back_inserter(convex_hull));
min_rectangle.clear();
CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));
min_parallelogram.clear();
CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));
std::vector<Point_2> center;
double radius;
CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
Vector_2 rvec(radius, radius);
for(std::size_t i = 0; i < center.size(); i++){
p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
}
}
int fcalc(float *volsph, Vec3i volsize,
int nnz, int nrays, Vec3i origin, int ri,
int *ptrs, int *cord, float *angtrs, int nang,
float *rhs, NUMBER *fval)
{
int nx, ny, nz, jglb, ierr;
float phi, theta, psi, sx, sy;
NUMBER cphi, sphi, cthe, sthe, cpsi, spsi;
float dm[8];
float * rvec;
*fval = 0.0;
nx = volsize[0];
ny = volsize[1];
nz = volsize[2];
rvec = new float[nx*ny];
for (int j = 0; j < nang; j++) {
for (int i = 0; i<nx*ny; i++) {
rvec[i] = 0.0;
}
// rvec <-- P(theta)*f
phi = angtrs[5*j+0];
theta = angtrs[5*j+1];
psi = angtrs[5*j+2];
sx = angtrs[5*j+3];
sy = angtrs[5*j+4];
cphi=cos(phi);
sphi=sin(phi);
cthe=cos(theta);
sthe=sin(theta);
cpsi=cos(psi);
spsi=sin(psi);
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
dm[6]=sx;
dm[7]=sy;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord, volsph, rvec);
// rvec <--- rvec - rhs
for (int i = 1; i <= nx*ny; i++) {
rvec(i) = rvec(i) - rhs(i,j+1);
}
// fval <--- norm(rvec)^2/2
for (int i = 0; i < nx*ny; i++) {
*fval += rvec[i]*rvec[i];
}
}
EMDeleteArray(rvec);
return 0;
}
bool ReconstructionHandler::initializeStereoModel(std::vector<cv::Point3d> &points, std::vector<cv::Vec3b> &pointsRGB, int imageInitializedCnt){
// Camera parameters - necessary for reconstruction
cv::Matx34d P0,P1;
cv::Mat_<double> t;
cv::Mat_<double> R;
cv::Mat_<double> rvec(1,3);
// Set arbitrary parameters - will be refined
P0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
P1 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
// Attempt to find baseline traingulation
LOG(Debug, "Trying to find baseline triangulation...");
// Find best two images to start with
std::vector<CloudPoint> tmpPcloud;
// Sort pairwise matches to find the lowest Homography inliers [Snavely07 4.2]
// Take less matches tan Snavely suggests...
std::list<std::pair<int,std::pair<int,int> > > matchesSizes;
// For debugging
std::map<std::pair<int,int> ,std::vector<cv::DMatch> > tempMatches = sceneModel->getMatches();
for(std::map<std::pair<int,int> ,std::vector<cv::DMatch> >::iterator i = tempMatches.begin(); i != tempMatches.end(); ++i) {
if((*i).second.size() < 60)
matchesSizes.push_back(make_pair(100,(*i).first));
else {
int Hinliers = findHomographyInliers2Views((*i).first.first,(*i).first.second);
int percent = (int)(((double)Hinliers) / ((double)(*i).second.size()) * 100.0);
LOG(Info, "The percentage of inliers for images ", (*i).first.first, "," , (*i).first.second, " = ", percent);
matchesSizes.push_back(make_pair((int)percent,(*i).first));
}
}
matchesSizes.sort(sortByFirst);
// Reconstruct from two views
bool goodF = false;
int highest_pair = 0;
m_first_view = m_second_view = 0;
// Reverse iterate by number of matches
for(std::list<std::pair<int,std::pair<int,int> > >::iterator highest_pair = matchesSizes.begin();
highest_pair != matchesSizes.end() && !goodF;
++highest_pair)
{
m_second_view = (*highest_pair).second.second;
m_first_view = (*highest_pair).second.first;
LOG(Debug, "Attempting reconstruction from view ", m_first_view, " and ", m_second_view);
// If reconstrcution of first two views is bad, fallback to another pair
// See if the Fundamental Matrix between these two views is good
std::vector<cv::KeyPoint> keypoints1Refined;
std::vector<cv::KeyPoint> keypoints2Refined;
goodF = findCameraMatrices(sceneModel->K, sceneModel->Kinv, sceneModel->distortionCoefficients,
sceneModel->getKeypoints(m_first_view), sceneModel->getKeypoints(m_second_view),
keypoints1Refined, keypoints2Refined, P0, P1, sceneModel->getMatches(m_first_view,m_second_view),
tmpPcloud);
if (goodF) {
std::vector<CloudPoint> new_triangulated;
std::vector<int> add_to_cloud;
sceneModel->poseMats[m_first_view] = P0;
sceneModel->poseMats[m_second_view] = P1;
// TODO imageInitialzedCnt used to initalize correspondence matching vector
// Will fail if more images are added later...
bool good_triangulation = triangulatePointsBetweenViews(m_second_view,m_first_view,new_triangulated,add_to_cloud, imageInitializedCnt);
if(!good_triangulation || cv::countNonZero(add_to_cloud) < 10) {
LOG(Debug, " Triangulation failed");
goodF = false;
sceneModel->poseMats[m_first_view] = 0;
sceneModel->poseMats[m_second_view] = 0;
m_second_view++;
} else {
assert(new_triangulated[0].imgpt_for_img.size() > 0);
LOG(Debug, " Points before triangulation: ", (int)sceneModel->reconstructedPts.size());
for (unsigned int j=0; j<add_to_cloud.size(); j++) {
if(add_to_cloud[j] == 1) {
sceneModel->reconstructedPts.push_back(new_triangulated[j]);
}
}
LOG(Debug, " Points after triangulation: ", (int)sceneModel->reconstructedPts.size());
}
}
}
if (!goodF) {
LOG(Error, "Cannot find a good pair of images to obtain a baseline triangulation");
return false;
}
LOG(Debug, "===================================================================");
LOG(Debug, "Taking baseline from " , m_first_view , " and " , m_second_view);
LOG(Debug, "===================================================================");
// Adjust bundle for everything so far before display
cv::Mat temporaryCameraMatrix = sceneModel->K;
BundleAdjustment BA;
BA.adjustBundle(sceneModel->reconstructedPts,temporaryCameraMatrix,sceneModel->getKeypoints(),sceneModel->poseMats);
updateReprojectionErrors();
#ifdef __DEBUG__DISPLAY__
// DEBUG - Drawing matches that survived the fundamental matrix
cv::drawMatches(sceneModel->frames[0], sceneModel->getKeypoints(0),
sceneModel->frames[1], sceneModel->getKeypoints(1),
sceneModel->getMatches(0,1), img_matches, cv::Scalar(0,0,255));
imshow( "Good Matches After F Refinement", img_matches );
cv::waitKey(0);
#endif
// Pass reconstructed points to 3D display
// TODO unnecessary copying
for (int i = 0; i < sceneModel->reconstructedPts.size(); ++i) {
points.push_back(sceneModel->reconstructedPts[i].pt);
}
getRGBForPointCloud(sceneModel->reconstructedPts, pointsRGB);
// End of stereo initialization
LOG(Debug, "Initialized stereo model");
P1 = sceneModel->poseMats[m_second_view];
t = (cv::Mat_<double>(1,3) << P1(0,3), P1(1,3), P1(2,3));
R = (cv::Mat_<double>(3,3) << P1(0,0), P1(0,1), P1(0,2),
P1(1,0), P1(1,1), P1(1,2),
P1(2,0), P1(2,1), P1(2,2));
Rodrigues(R, rvec);
// Update sets which track what was processed
sceneModel->doneViews.clear();
sceneModel->goodViews.clear();
sceneModel->doneViews.insert(m_first_view);
sceneModel->doneViews.insert(m_second_view);
sceneModel->goodViews.insert(m_first_view);
sceneModel->goodViews.insert(m_second_view);
return true;
}
int fgcalc(MPI_Comm comm, float *volsph, Vec3i volsize,
int nnz, int nrays, Vec3i origin, int ri,
int *ptrs, int *cord, float *angtrs, int nang,
float *rhs, float aba, NUMBER *fval, float *grad, char * fname_base)
{
int mypid, ncpus, nvars;
int *psize, *nbase;
int nangloc, nx, ny, nz, jglb, ierr, ibeg;
float phi, theta, psi, sx, sy;
NUMBER cphi, sphi, cthe, sthe, cpsi, spsi;
float dm[8];
float *rvec, *gvec1, *gvec2, *gvec3, *gvec4, *gvec5, *gradloc;
NUMBER fvalloc= 0.0;
MPI_Comm_rank(comm,&mypid);
MPI_Comm_size(comm,&ncpus);
nvars = nnz + nang*5;
*fval = 0.0;
for (int i = 0; i < nvars; i++) grad[i]=0.0;
nx = volsize[0];
ny = volsize[1];
nz = volsize[2];
psize = new int[ncpus];
nbase = new int[ncpus];
rvec = new float[nx*ny];
gvec1 = new float[nx*ny];
gvec2 = new float[nx*ny];
gvec3 = new float[nx*ny];
gvec4 = new float[nx*ny];
gvec5 = new float[nx*ny];
gradloc = new float[nvars];
// float ref_params[5]; // Phi:: stores matrix parameters so they won't get +dt/-dt jitters
// Could do this with just one float instead of an array
float ref_param; // Phi:: stores matrix parameters so they won't get +dt/-dt jitters
// ref_param shouldn't be needed, but it seemed that there were some strange things going on nonetheless...
// Phi:: gradloc was not initialized
for ( int i = 0 ; i < nvars ; ++i ) {
gradloc[i] = 0.0;
}
std::ofstream res_out;
char out_fname[64];
sprintf(out_fname, "res_%s.dat", fname_base);
res_out.open(out_fname);
nangloc = setpart(comm, nang, psize, nbase);
float * img_res = new float[nangloc]; // here we'll store the residuals for each data image
dm[6] = 0.0;
dm[7] = 0.0;
for (int j = 0; j < nangloc; j++) {
img_res[j] = 0.0; // initialize; in the inner loop we accumulate the residual at each pixel
for (int i = 0; i<nx*ny; i++) {
rvec[i] = 0.0;
gvec1[i] = 0.0;
gvec2[i] = 0.0;
gvec3[i] = 0.0;
gvec4[i] = 0.0;
gvec5[i] = 0.0;
}
// rvec <-- P(theta)*f
jglb = nbase[mypid] + j;
phi = angtrs[5*jglb+0];
theta = angtrs[5*jglb+1];
psi = angtrs[5*jglb+2];
sx = angtrs[5*jglb+3];
sy = angtrs[5*jglb+4];
cphi=cos(phi);
sphi=sin(phi);
cthe=cos(theta);
sthe=sin(theta);
cpsi=cos(psi);
spsi=sin(psi);
// sincos(phi, &sphi, &cphi);
// sincos(theta, &sthe, &cthe);
// sincos(psi, &spsi, &cpsi);
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
dm[6]=sx;
dm[7]=sy;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord, volsph, rvec);
// gvec1 <--- P(phi+dt)*f
ref_param = phi;
phi = phi + dt;
// sincos(phi, &sphi, &cphi);
cphi = cos(phi);
sphi = sin(phi);
phi = ref_param;
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
volsph, gvec1);
// phi = phi - dt;
// phi = ref_params[0];
cphi = cos(phi);
sphi = sin(phi);
// sincos(phi, &sphi, &cphi);
// gvec1 <--- (gvec1 - rvec)/dt
for (int i = 0; i<nx*ny; i++) gvec1[i] = (gvec1[i] - rvec[i])/dt;
// gvec2 <--- P(theta+dt)*f
ref_param = theta;
theta = theta + dt;
cthe = cos(theta);
sthe = sin(theta);
// sincos(theta, &sthe, &cthe);
theta = ref_param;
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
volsph, gvec2);
// theta = theta - dt;
// theta = ref_params[1];
cthe = cos(theta);
sthe = sin(theta);
// sincos(theta, &sthe, &cthe);
// gvec2 <--- (gvec2 - rvec)/dt
for (int i = 0; i<nx*ny; i++) gvec2[i] = (gvec2[i]-rvec[i])/dt;
// gvec3 <--- P(psi+dt)*f
ref_param = psi;
psi = psi + dt;
cpsi = cos(psi);
spsi = sin(psi);
// sincos(psi, &spsi, &cpsi);
psi = ref_param;
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
volsph, gvec3);
// psi = psi - dt;
// psi = ref_params[2];
cpsi = cos(psi);
spsi = sin(psi);
// sincos(psi, &spsi, &cpsi);
for (int i = 0; i < nx*ny; i++) gvec3[i] = (gvec3[i] - rvec[i])/dt;
// Phi:: Needed to put this in too; still working with shifted psi in dm before
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
// gvec4 <--- P(phi,theta,psi)*f(sx+dt,sy)
//Phi:: pass shift in the last two entries of dm
ref_param = dm[6];
dm[6] += dt;
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
volsph, gvec4);
// dm[6] = ref_params[3];
dm[6] = ref_param;
// gvec4 <--- (gvec4 - rvec)/dt
for (int i = 0; i < nx*ny; i++) gvec4[i] = (gvec4[i]-rvec[i])/dt;
ref_param = dm[7];
dm[7] += dt;
// gvec5 <--- P(phi,theta,psi)*f(sx,sy+dt)
// Phi:: changed typo: last arg was gvec4, is now gvec5
ierr = fwdpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
volsph, gvec5);
// dm[7] = ref_params[4];
dm[7] = ref_param;
// gvec5 <--- (gvec5 - rvec)/dt
for (int i = 0; i < nx*ny; i++) gvec5[i] = (gvec5[i]-rvec[i])/dt;
// std::cout << "\n" << rhs(1,j+1) << "\n" << std::endl;
// rvec <--- rvec - rhs
for (int i = 1; i <= nx*ny; i++) {
rvec(i) = rvec(i) - rhs(i,j+1);
}
// std::cout << "\n" << rvec[0] << "\n" << std::endl;
ierr = bckpj3(volsize, nrays, nnz, dm, origin, ri, ptrs, cord,
rvec, gradloc);
// fval <--- norm(rvec)^2/2
// ibeg = nnz + 5*(jglb-1);
// Phi:: Indexing error, the -1 should be outside the parenths
// also, the parenths can be removed
ibeg = nnz + 5*(jglb) - 1;
for (int i = 0; i < nx*ny; i++) {
fvalloc += rvec[i]*rvec[i];
img_res[j] += rvec[i] * rvec[i];
gradloc(ibeg+1) += rvec[i]*gvec1[i];
gradloc(ibeg+2) += rvec[i]*gvec2[i];
gradloc(ibeg+3) += rvec[i]*gvec3[i];
gradloc(ibeg+4) += rvec[i]*gvec4[i];
gradloc(ibeg+5) += rvec[i]*gvec5[i];
}
//
}
ierr = MPI_Allreduce(gradloc, grad, nvars, MPI_FLOAT, MPI_SUM, comm);
ierr = MPI_Allreduce(&fvalloc, fval, 1, MPI_FLOAT, MPI_SUM, comm);
// // in turn, send each array of image residuals to master, then write to disk
MPI_Status mpistatus;
if (mypid == 0) {
for ( int j = 0 ; j < psize[0] ; ++j ) {
res_out << std::scientific << img_res[j] << std::endl;
}
for ( int j = 1 ; j < ncpus ; ++j ) {
ierr = MPI_Recv(img_res, psize[j], MPI_FLOAT, j, j, comm, &mpistatus);
for ( int i = 0 ; i < psize[j] ; ++i ) {
res_out << std::scientific << img_res[i] << std::endl;
}
}
} else { // mypid != 0 , send my data to master to write out
ierr = MPI_Send(img_res, nangloc, MPI_FLOAT, 0, mypid, comm);
}
res_out.close();
EMDeleteArray(psize);
EMDeleteArray(nbase);
EMDeleteArray(rvec);
EMDeleteArray(gvec1);
EMDeleteArray(gvec2);
EMDeleteArray(gvec3);
EMDeleteArray(gvec4);
EMDeleteArray(gvec5);
EMDeleteArray(gradloc);
EMDeleteArray(img_res);
return 0;
}
void SolvePNP_method(vector <Point3f> points3d,
vector <Point2f> imagePoints,
MatrixXf &R,Vector3f &t, MatrixXf &xcam_,
MatrixXf K,
const PointCloud<PointXYZRGB>::Ptr& cloudRGB,//projected data
const PointCloud<PointXYZ>::Ptr& cloud_laser_cord,//laser coordinates
MatrixXf& points_projected,Mat image,
PointCloud<PointXYZ>::ConstPtr cloud)
{
Mat_<float> camera_matrix (3, 3);
camera_matrix(0,0)=K(0,0);
camera_matrix(0,1)=K(0,1);
camera_matrix(0,2)=K(0,2);
camera_matrix(1,0)=K(1,0);
camera_matrix(1,1)=K(1,1);
camera_matrix(1,2)=K(1,2);
camera_matrix(2,0)=K(2,0);
camera_matrix(2,1)=K(2,1);
camera_matrix(2,2)=K(2,2);
vector<double> rv(3), tv(3);
Mat rvec(rv),tvec(tv);
Mat_<float> dist_coef (5, 1);
dist_coef = 0;
cout <<camera_matrix<<endl;
cout<< imagePoints<<endl;
cout<< points3d <<endl;
solvePnP( points3d, imagePoints, camera_matrix, dist_coef, rvec, tvec);
//////////////////////////////////////////////////7
//convert data
//////////////////////////////////////////////////7
double rot[9] = {0};
Mat R_2(3,3,CV_64FC1,rot);
//change results only debugging
Rodrigues(rvec, R_2);
R=Matrix3f::Zero(3,3);
t=Vector3f(0,0,0);
double* _r = R_2.ptr<double>();
double* _t = tvec.ptr<double>();
t(0)=_t[0];
t(1)=_t[1];
t(2)=_t[2];
R(0,0)=_r[0];
R(0,1)=_r[1];
R(0,2)=_r[2];
R(1,0)=_r[3];
R(1,1)=_r[4];
R(1,2)=_r[5];
R(2,0)=_r[6];
R(2,1)=_r[7];
R(2,2)=_r[8];
points_projected=MatrixXf::Zero(2,cloud->points.size ());
for (int j=0;j<cloud->points.size ();j++){
PointCloud<PointXYZRGB> PointAuxRGB;
PointAuxRGB.points.resize(1);
Vector3f xw=Vector3f(cloud->points[j].x,
cloud->points[j].y,
cloud->points[j].z);
xw=K*( R*xw+t);
xw= xw/xw(2);
int col,row;
col=(int)xw(0);
row=(int)xw(1);
points_projected(0,j)=(int)xw(0);
points_projected(1,j)=(int)xw(1);
int b,r,g;
if ((col<0 || row<0) || (col>image.cols || row>image.rows)){
r=0;
g=0;
b=0;
}else{
// b = img->imageData[img->widthStep * row+ col * 3];
// g = img->imageData[img->widthStep * row+ col * 3 + 1];
//r = img->imageData[img->widthStep * row+ col * 3 + 2];
r=image.at<cv::Vec3b>(row,col)[0];
g=image.at<cv::Vec3b>(row,col)[1];
b=image.at<cv::Vec3b>(row,col)[2];
//std::cout << image.at<cv::Vec3b>(row,col)[0] << " " << image.at<cv::Vec3b>(row,col)[1] << " " << image.at<cv::Vec3b>(row,col)[2] << std::endl;
}
//std::cout << "( "<<r <<","<<g<<","<<b<<")"<<std::endl;
uint8_t r_i = r;
uint8_t g_i = g;
uint8_t b_i = b;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
//int32_t rgb = (r_i << 16) | (g_i << 8) | b_i;
PointAuxRGB.points[0].x=cloud->points[j].x;
PointAuxRGB.points[0].y=cloud->points[j].y;
PointAuxRGB.points[0].z=cloud->points[j].z;
PointAuxRGB.points[0].rgb=*reinterpret_cast<float*>(&rgb);
cloudRGB->points.push_back (PointAuxRGB.points[0]);
//project point to the image
}
}
bool ReconstructionHandler::addNextViewToStereoModel(std::vector<cv::Point3d> &points, std::vector<cv::Vec3b> &pointsRGB, int imageInitializedCnt){
// Camera parameters - necessary for reconstruction
cv::Matx34d P0,P1;
cv::Mat_<double> t;
cv::Mat_<double> R;
cv::Mat_<double> rvec(1,3);
// Find image with highest 2d-3d correspondance [Snavely07 4.2]
int max_2d3d_view = -1;
int max_2d3d_count = 0;
std::vector<cv::Point3f> _3dPoints;
std::vector<cv::Point2f> _2dPoints;
for (unsigned int _i=0; _i < imageInitializedCnt; _i++) {
if(sceneModel->doneViews.find(_i) != sceneModel->doneViews.end()) continue; //already done with this view
std::vector<cv::Point3f> tmp3d; std::vector<cv::Point2f> tmp2d;
find2D3DCorrespondences(_i,tmp3d,tmp2d);
if(tmp3d.size() > max_2d3d_count) {
max_2d3d_count = tmp3d.size();
max_2d3d_view = _i;
_3dPoints = tmp3d; _2dPoints = tmp2d;
}
}
int i = max_2d3d_view; //highest 2d3d matching view
LOG(Debug, "Image with most 2D-3D correspondences is ", i, " with ", max_2d3d_count);
sceneModel->doneViews.insert(i);
// Find pose of camera for new image based on these correspondences
bool poseEstimated = findPoseEstimation(i,rvec,t,R,_3dPoints,_2dPoints);
if(!poseEstimated){
LOG (Warn, "Pose estimation for view ", i , "failed. Continuing.");
return false;
}
// Store estimated pose
sceneModel->poseMats[i] = cv::Matx34d (R(0,0),R(0,1),R(0,2),t(0),
R(1,0),R(1,1),R(1,2),t(1),
R(2,0),R(2,1),R(2,2),t(2));
// Triangulate current view with all previous good views to reconstruct more 3D points
// Good views are those views for which camera poses were calculated successfully
//...
for (std::set<int>::iterator goodView = sceneModel->goodViews.begin(); goodView != sceneModel->goodViews.end(); ++goodView)
{
int view = *goodView;
if( view == i ) continue; // Skip current
std::vector<CloudPoint> newTriangulated;
std::vector<int> addToCloud;
bool goodTriangulation = triangulatePointsBetweenViews(i,view,newTriangulated,addToCloud, imageInitializedCnt);
if(!goodTriangulation){
LOG(Warn, "Triangulating current view with view ", view, " has failed! Continuing.");
continue;
}
LOG(Info, "Number of points before triangulation ", (int)sceneModel->reconstructedPts.size());
for (int j=0; j<addToCloud.size(); j++) {
if(addToCloud[j] == 1)
sceneModel->reconstructedPts.push_back(newTriangulated[j]);
}
LOG(Info, "Number of points after triangulation ", (int)sceneModel->reconstructedPts.size());;
}
sceneModel->goodViews.insert(i);
// Bundle adjustment for all views & points reconstructed so far
cv::Mat temporaryCameraMatrix = sceneModel->K;
BundleAdjustment BA;
BA.adjustBundle(sceneModel->reconstructedPts,temporaryCameraMatrix,sceneModel->getKeypoints(),sceneModel->poseMats);
updateReprojectionErrors();
// Pass reconstructed points to 3D display
// TODO unnecessary copying
for (int i = 0; i < sceneModel->reconstructedPts.size(); ++i) {
points.push_back(sceneModel->reconstructedPts[i].pt);
}
getRGBForPointCloud(sceneModel->reconstructedPts, pointsRGB);
return true;
}
w_rc_t ShoreTPCCEnv::_post_init_impl()
{
#ifndef CFG_HACK
return (RCOK);
#endif
TRACE (TRACE_ALWAYS, "Padding WAREHOUSES");
ss_m* db = this->db();
// lock the WH table
warehouse_t* wh = warehouse_desc();
index_desc_t* idx = wh->indexes();
int icount = wh->index_count();
stid_t wh_fid = wh->fid();
// lock the table and index(es) for exclusive access
W_DO(db->lock(wh_fid, EX));
for(int i=0; i < icount; i++) {
for(int j=0; j < idx[i].get_partition_count(); j++)
W_DO(db->lock(idx[i].fid(j), EX));
}
guard<ats_char_t> pts = new ats_char_t(wh->maxsize());
/* copy and pad all tuples smaller than 4k
WARNING: this code assumes that existing tuples are packed
densly so that all padded tuples are added after the last
unpadded one
*/
bool eof;
static int const PADDED_SIZE = 4096; // we know you can't fit two 4k records on a single page
array_guard_t<char> padding = new char[PADDED_SIZE];
std::vector<rid_t> hit_list;
{
guard<warehouse_man_impl::table_iter> iter;
{
warehouse_man_impl::table_iter* tmp;
W_DO(warehouse_man()->get_iter_for_file_scan(db, tmp));
iter = tmp;
}
int count = 0;
table_row_t row(wh);
rep_row_t arep(pts);
int psize = wh->maxsize()+1;
W_DO(iter->next(db, eof, row));
while (1) {
pin_i* handle = iter->cursor();
if (!handle) {
TRACE(TRACE_ALWAYS, " -> Reached EOF. Search complete (%d)\n", count);
break;
}
// figure out how big the old record is
int hsize = handle->hdr_size();
int bsize = handle->body_size();
if (bsize == psize) {
TRACE(TRACE_ALWAYS, " -> Found padded WH record. Stopping search (%d)\n", count);
break;
}
else if (bsize > psize) {
// too big... shrink it down to save on logging
handle->truncate_rec(bsize - psize);
fprintf(stderr, "+");
}
else {
// copy and pad the record (and mark the old one for deletion)
rid_t new_rid;
vec_t hvec(handle->hdr(), hsize);
vec_t dvec(handle->body(), bsize);
vec_t pvec(padding, PADDED_SIZE-bsize);
W_DO(db->create_rec(wh_fid, hvec, PADDED_SIZE, dvec, new_rid));
W_DO(db->append_rec(new_rid, pvec));
// for small databases, first padded record fits on this page
if (not handle->up_to_date())
handle->repin();
// mark the old record for deletion
hit_list.push_back(handle->rid());
// update the index(es)
vec_t rvec(&row._rid, sizeof(rid_t));
vec_t nrvec(&new_rid, sizeof(new_rid));
for(int i=0; i < icount; i++) {
int key_sz = warehouse_man()->format_key(idx+i, &row, arep);
vec_t kvec(arep._dest, key_sz);
/* destroy the old mapping and replace it with the new
one. If it turns out this is super-slow, we can
look into probing the index with a cursor and
updating it directly.
*/
int pnum = _pwarehouse_man->get_pnum(&idx[i], &row);
stid_t fid = idx[i].fid(pnum);
if(idx[i].is_mr()) {
W_DO(db->destroy_mr_assoc(fid, kvec, rvec));
// now put the entry back with the new rid
el_filler ef;
ef._el.put(nrvec);
W_DO(db->create_mr_assoc(fid, kvec, ef));
} else {
W_DO(db->destroy_assoc(fid, kvec, rvec));
// now put the entry back with the new rid
W_DO(db->create_assoc(fid, kvec, nrvec));
}
}
fprintf(stderr, ".");
}
// next!
count++;
W_DO(iter->next(db, eof, row));
}
fprintf(stderr, "\n");
// put the iter out of scope
}
// delete the old records
int hlsize = hit_list.size();
TRACE(TRACE_ALWAYS, "-> Deleting (%d) old unpadded records\n", hlsize);
for(int i=0; i < hlsize; i++) {
W_DO(db->destroy_rec(hit_list[i]));
}
return (RCOK);
}
void Calib::computeCameraPosePnP()
{
std::cout << "There are " << imagePoints.size() << " imagePoints and " << objectPoints.size() << " objectPoints." << std::endl;
cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
cv::setIdentity(cameraMatrix);
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 3; j++)
{
cameraMatrix.at<double> (i, j) = M(i, j);
}
}
std::cout << "Initial cameraMatrix: " << cameraMatrix << std::endl;
cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
std::vector<cv::Point3d> objectPoints_cv;
std::vector<cv::Point2d> imagePoints_cv;
for(int point_id = 0;point_id < objectPoints.size();point_id++)
{
cv::Point3d object_point;
cv::Point2d image_point;
object_point.x = objectPoints[point_id](0);
object_point.y = objectPoints[point_id](1);
object_point.z = objectPoints[point_id](2);
image_point.x = imagePoints[point_id](0);
image_point.y = imagePoints[point_id](1);
objectPoints_cv.push_back(object_point);
imagePoints_cv.push_back(image_point);
}
cv::solvePnP(objectPoints_cv, imagePoints_cv, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "rvec: " << rvec << std::endl;
std::cout << "tvec: " << tvec << std::endl;
std::vector<cv::Point2d> projectedPoints;
cv::projectPoints(objectPoints_cv, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
}
//inverting the R|t to get camera position in world co-ordinates
cv::Mat Rot(3,3,cv::DataType<double>::type);
cv::Rodrigues(rvec, Rot);
Rot = Rot.t();
tvec = -Rot*tvec;
this->R << Rot.at<double>(0,0), Rot.at<double>(0,1), Rot.at<double>(0,2), Rot.at<double>(1,0), Rot.at<double>(1,1), Rot.at<double>(1,2), Rot.at<double>(2,0), Rot.at<double>(2,1), Rot.at<double>(2,2);
this->T << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
}
w_rc_t ShoreTPCBEnv::_pad_BRANCHES()
{
ss_m* db = this->db();
// lock the BRANCHES table
branch_t* br = branch_man->table();
std::vector<index_desc_t*>& br_idx = br->get_indexes();
// lock the table and index(es) for exclusive access
W_DO(ss_m::lm->intent_vol_lock(br->primary_idx()->stid().vol,
okvl_mode::IX));
W_DO(ss_m::lm->intent_store_lock(br->primary_idx()->stid(),
okvl_mode::X));
for(size_t i=0; i < br_idx.size(); i++) {
W_DO(ss_m::lm->intent_store_lock(br_idx[i]->stid(), okvl_mode::X));
}
guard<ats_char_t> pts = new ats_char_t(br->maxsize());
// copy and pad all tuples smaller than 4k
// WARNING: this code assumes that existing tuples are packed
// densly so that all padded tuples are added after the last
// unpadded one
bool eof;
// we know you can't fit two 4k records on a single page
static int const PADDED_SIZE = 4096;
array_guard_t<char> padding = new char[PADDED_SIZE];
std::vector<rid_t> hit_list;
{
table_scan_iter_impl<branch_t>* iter =
new table_scan_iter_impl<branch_t>(branch_man->table());
int count = 0;
table_row_t row(br);
rep_row_t arep(pts);
int psize = br->maxsize()+1;
W_DO(iter->next(db, eof, row));
while (!eof) {
// figure out how big the old record is
int bsize = row.size();
if (bsize == psize) {
TRACE(TRACE_ALWAYS,
"-> Found padded BRANCH record. Stopping search (%d)\n",
count);
break;
}
else if (bsize > psize) {
// too big... shrink it down to save on logging
// handle->truncate_rec(bsize - psize);
fprintf(stderr, "+");
// CS: no more pin_i -> do nothing
}
else {
// copy and pad the record (and mark the old one for deletion)
rid_t new_rid;
vec_t hvec(handle->hdr(), hsize);
vec_t dvec(handle->body(), bsize);
vec_t pvec(padding, PADDED_SIZE-bsize);
W_DO(db->create_rec(br_fid, hvec, PADDED_SIZE, dvec, new_rid));
W_DO(db->append_rec(new_rid, pvec));
// mark the old record for deletion
hit_list.push_back(handle->rid());
// update the index(es)
vec_t rvec(&row._rid, sizeof(rid_t));
vec_t nrvec(&new_rid, sizeof(new_rid));
for(int i=0; i < br_idx_count; i++) {
int key_sz = branch_man()->format_key(br_idx+i, &row, arep);
vec_t kvec(arep._dest, key_sz);
// destroy the old mapping and replace it with the new
// one. If it turns out this is super-slow, we can
// look into probing the index with a cursor and
// updating it directly.
int pnum = _pbranch_man->get_pnum(&br_idx[i], &row);
stid_t fid = br_idx[i].fid(pnum);
W_DO(db->destroy_assoc(fid, kvec, rvec));
// now put the entry back with the new rid
W_DO(db->create_assoc(fid, kvec, nrvec));
}
fprintf(stderr, ".");
}
// next!
count++;
W_DO(iter->next(db, eof, row));
}
TRACE(TRACE_ALWAYS, "padded records added\n");
delete iter;
}
// delete the old records
int hlsize = hit_list.size();
TRACE(TRACE_ALWAYS,
"-> Deleting (%d) old BRANCH unpadded records\n",
hlsize);
for(int i=0; i < hlsize; i++) {
W_DO(db->destroy_rec(hit_list[i]));
}
return (RCOK);
}
// Determines if pos is inside the domain
bool pDomain::Within(const pVector &pos) const
{
switch (type)
{
case PDBox:
return !((pos.x < p1.x) || (pos.x > p2.x) ||
(pos.y < p1.y) || (pos.y > p2.y) ||
(pos.z < p1.z) || (pos.z > p2.z));
case PDPlane:
// Distance from plane = n * p + d
// Inside is the positive half-space.
return pos * p2 >= -radius1;
case PDSphere:
{
pVector rvec(pos - p1);
float rSqr = rvec.length2();
return rSqr <= radius1Sqr && rSqr >= radius2Sqr;
}
case PDCylinder:
case PDCone:
{
// This is painful and slow. Might be better to do quick
// accept/reject tests.
// Let p2 = vector from base to tip of the cylinder
// x = vector from base to test point
// x . p2
// dist = ------ = projected distance of x along the axis
// p2. p2 ranging from 0 (base) to 1 (tip)
//
// rad = x - dist * p2 = projected vector of x along the base
// p1 is the apex of the cone.
pVector x(pos - p1);
// Check axial distance
// radius2Sqr stores 1 / (p2.p2)
float dist = (p2 * x) * radius2Sqr;
if(dist < 0.0f || dist > 1.0f)
return false;
// Check radial distance; scale radius along axis for cones
pVector xrad = x - p2 * dist; // Radial component of x
float rSqr = xrad.length2();
if(type == PDCone)
return (rSqr <= fsqr(dist * radius1) &&
rSqr >= fsqr(dist * radius2));
else
return (rSqr <= radius1Sqr && rSqr >= fsqr(radius2));
}
case PDBlob:
{
pVector x(pos - p1);
// return exp(-0.5 * xSq * Sqr(oneOverSigma)) * ONEOVERSQRT2PI * oneOverSigma;
float Gx = expf(x.length2() * radius2Sqr) * radius2;
return (drand48() < Gx);
}
case PDPoint:
case PDLine:
case PDRectangle:
case PDTriangle:
case PDDisc:
default:
return false; // XXX Is there something better?
}
}
void CalcStatistics::calculate() {
CLOG(LDEBUG)<<"in calculate()";
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rvec;
cv::Mat_<double> tvec;
cv::Mat_<double> axis;
cv::Mat_<double> rotMatrix;
float fi;
rotMatrix= cv::Mat_<double>::zeros(3,3);
tvec = cv::Mat_<double>::zeros(3,1);
axis = cv::Mat_<double>::zeros(3,1);
// first homogMatrix on InputStream
if(counter == 0) {
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
cumulatedHomogMatrix = homogMatrix;
cumulatedTvec = tvec;
cumulatedRvec = rvec;
fi = sqrt((pow(rvec(0,0), 2) + pow(rvec(1,0), 2)+pow(rvec(2,0),2)));
cumulatedFi = fi;
for(int k=0;k<3;k++) {
axis(k,0)=rvec(k,0)/fi;
}
cumulatedAxis = axis;
counter=1;
return;
}
homogMatrix=in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
fi = sqrt((pow(rvec(0,0), 2) + pow(rvec(1,0), 2)+pow(rvec(2,0),2)));
for(int k=0;k<3;k++) {
axis(k,0)=rvec(k,0)/fi;
}
cumulatedFi += fi;
cumulatedTvec += tvec;
cumulatedRvec += rvec;
cumulatedAxis += axis;
counter++;
avgFi = cumulatedFi/counter;
avgAxis = cumulatedAxis/counter;
avgRvec = avgAxis * avgFi;
avgTvec = cumulatedTvec/counter;
Types::HomogMatrix hm;
cv::Mat_<double> rottMatrix;
Rodrigues(avgRvec, rottMatrix);
CLOG(LINFO)<<"Uśredniona macierz z "<<counter<<" macierzy \n";
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
hm(i, j) = rottMatrix(i, j);
CLOG(LINFO) << hm(i, j) << " ";
}
hm(i, 3) = avgTvec(i, 0);
CLOG(LINFO) << hm(i, 3) <<" \n";
}
out_homogMatrix.write(hm);
CLOG(LINFO)<<"Uśredniony kąt: " << avgFi;
float standardDeviationFi = sqrt(pow(avgFi - fi, 2));
CLOG(LINFO)<<"Odchylenie standardowe kąta: "<<standardDeviationFi;
}
void DrawCoordinateSystem::projectPoints(){
cv::Mat_<double> rvec(3,1);
cv::Mat_<double> tvec(3,1);
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rotMatrix;
rotMatrix.create(3,3);
// Try to read rotation and translation from rvec and tvec or homogenous matrix.
if(!in_rvec.empty()&&!in_tvec.empty()){
rvec = in_rvec.read();
tvec = in_tvec.read();
}
else if(!in_homogMatrix.empty()){
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
}
else
return;
// Get camera info properties.
Types::CameraInfo camera_matrix = in_camera_matrix.read();
vector<cv::Point3f> object_points;
vector<cv::Point2f> image_points;
// Create four points constituting ends of three lines.
object_points.push_back(cv::Point3f(0,0,0));
object_points.push_back(cv::Point3f(0.1,0,0));
object_points.push_back(cv::Point3f(0,0.1,0));
object_points.push_back(cv::Point3f(0,0,0.1));
// Project points taking into account the camera properties.
if (rectified) {
cv::Mat K, _r, _t;
cv::decomposeProjectionMatrix(camera_matrix.projectionMatrix(), K, _r, _t);
cv::projectPoints(object_points, rvec, tvec, K, cv::Mat(), image_points);
} else {
cv::projectPoints(object_points, rvec, tvec, camera_matrix.cameraMatrix(), camera_matrix.distCoeffs(), image_points);
}
// Draw lines between projected points.
Types::Line ax(image_points[0], image_points[1]);
ax.setCol(cv::Scalar(255, 0, 0));
Types::Line ay(image_points[0], image_points[2]);
ay.setCol(cv::Scalar(0, 255, 0));
Types::Line az(image_points[0], image_points[3]);
az.setCol(cv::Scalar(0, 0, 255));
// Add lines to container.
Types::DrawableContainer ctr;
ctr.add(ax.clone());
ctr.add(ay.clone());
ctr.add(az.clone());
CLOG(LINFO)<<"Image Points: "<<image_points;
// Write output to dataports.
out_csystem.write(ctr);
out_impoints.write(image_points);
}
int main() {
// ## Make these inputs to the function ##
const char* camDataFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/PS3Eye_out_camera_data.yml";
const char* objPointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/LEDPos copy2.txt";
const char* imageFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/Test Images/Test3.jpg";
const char* imagePointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/imagePts3RANDOM"
".txt";
// Open the correct image
std::cout << imageFilename << std::endl << imagePointsFilename << std::endl;
cv::Mat img = cv::imread(imageFilename);
cv::namedWindow("Window");
cv::imshow("Window", img);
// Get the camera matrix and distortion coefficients
cv::Mat cameraMatrix, distCoeffs;
getCalibrationData(camDataFilename, cameraMatrix, distCoeffs);
// Solve pose estimation problem
// get the object points (3d) and image points (2d)
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
std::cout << "Object Points:" << std::endl;
readPoints(objPointsFilename, objectPoints);
std::cout << "\nImage Points:" << std::endl;
readPoints(imagePointsFilename, imagePoints);
// sort the image points
ledFinder(imagePoints,imagePoints,1);
std::cout << "\nSorted Image Points:" << std::endl;
std::cout << imagePoints << std::endl;
// Provide an initial guess: (give an approximate attitude--assume following from behind)
// (OpenCV reference frame) NOTE: rvec has weird quaternion convention, but [0,0,0]
// corresponds to following direclty behind
cv::Mat rvec(3,1,CV_64F);
cv::Mat tvec(3,1,CV_64F);
rvec.at<double>(0,0) = 0*DEG2RAD;
rvec.at<double>(1,0) = 0*DEG2RAD;
rvec.at<double>(2,0) = 0*DEG2RAD;
tvec.at<double>(0,0) = 0*IN2MM;
tvec.at<double>(1,0) = 0*IN2MM;
tvec.at<double>(2,0) = 100*IN2MM;
//
// solve for the pose that minimizes reprojection error
// UNITS: (objectPoints = mm, imagePoints = pixels, ... , rvec = radians, tvec = mm)
clock_t t;
t = clock();
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, 1, CV_EPNP);
t = clock() - t;
std::cout << "\nTime To Estimate Pose: " << ((float)t/CLOCKS_PER_SEC*1000) << " ms" << std::endl;
// compute the theta and r parts of the Euler rotation
/*
double rvec_theta = cv::norm(rvec, cv::NORM_L2);
cv::Mat rvec_r = rvec/rvec_theta;
*/
// get rotation matrix
cv::Mat rotMat, CV2B(3,3,CV_64F,cv::Scalar(0)), B2CV;
CV2B.at<double>(0,2) = 1.0;
CV2B.at<double>(1,0) = 1.0;
CV2B.at<double>(2,1) = 1.0;
cv::transpose(CV2B,B2CV); // CV2B and B2CV convert between OpenCV
cv::Rodrigues(rvec,rotMat); // frame convention and typical body frame
// extract phi, theta, psi (NOTE: these are for typical body frame)
double phi, theta, psi;
rotMat = CV2B * rotMat * B2CV; // change the rotation matrix from CV convention to RHS body frame
theta = asin(-rotMat.at<double>(2,0)); // get the Euler angles
psi = acos(rotMat.at<double>(0,0) / cos(theta));
phi = asin(rotMat.at<double>(2,1) / cos(theta));
// add changes to the projection for troubleshooting
phi += 0*DEG2RAD; // phi, theta, psi in body frame
theta += 0*DEG2RAD;
psi += 0*DEG2RAD;
tvec.at<double>(0,0) += 0*IN2MM; //tvec in OpenCV frame
tvec.at<double>(1,0) += 0*IN2MM;
tvec.at<double>(2,0) += 0*IN2MM;
// reconstruct rotation matrix: from object frame to camera frame
rotMat.at<double>(0,0) = cos(theta)*cos(psi);
rotMat.at<double>(0,1) = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
rotMat.at<double>(0,2) = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
rotMat.at<double>(1,0) = cos(theta)*sin(psi);
rotMat.at<double>(1,1) = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
rotMat.at<double>(1,2) = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
rotMat.at<double>(2,0) = -sin(theta);
rotMat.at<double>(2,1) = sin(phi)*cos(theta);
rotMat.at<double>(2,2) = cos(phi)*cos(theta);
// rewrite the rvec from rotation matrix
rotMat = B2CV * rotMat * CV2B; // convert RHS body rotation matrix to OpenCV rotation matrix
cv::Rodrigues(rotMat,rvec);
// print to standard output
cv::Mat tvec_body;
tvec_body.push_back(tvec.at<double>(2,0)); // get tvec in body coordinates to display to
tvec_body.push_back(tvec.at<double>(0,0)); // standard output
tvec_body.push_back(tvec.at<double>(1,0));
std::cout << "\nPhi, Theta, Psi: " << (phi*RAD2DEG) << ", " << (theta*RAD2DEG) <<
", " << (psi*RAD2DEG) << std::endl;
std::cout << "\ntvec:\n" << (tvec_body*MM2IN) << std::endl;
std::cout << "\nTotal Distance: " << (cv::norm(tvec,cv::NORM_L2)*MM2IN) << std::endl;
// compute the (re)projected points
cv::vector<cv::Point3f> axesPoints; // create points for coordinate axes
axesPoints.push_back(cv::Point3f(0,0,0));
axesPoints.push_back(cv::Point3f(AXES_LN,0,0));
axesPoints.push_back(cv::Point3f(0,AXES_LN,0));
axesPoints.push_back(cv::Point3f(0,0,AXES_LN));
cv::vector<cv::Point2f> projImagePoints, projAxesPoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projImagePoints);
cv::projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, projAxesPoints);
std::cout << "\nProjected Image Points:\n" << projImagePoints << std::endl;
// ## Need to compute the projected error to determine feasibility Decide on a threshold
double reprojErr = scaledReprojError(imagePoints, projImagePoints);
std::cout << "\nReprojection Error " << reprojErr << std::endl << std::endl;
// Plot the LED's and reprojected positions on the image
for (int i=0; i<NO_LEDS; i++) {
cv::circle(img,imagePoints[i], 3, cv::Scalar(0,0,255), 2);
cv::circle(img,projImagePoints[i], 3, cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[1], cv::Scalar(0,0,255), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[2], cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[3], cv::Scalar(255,0,0), 2);
}
cv::imshow("Window",img);
//cv::imwrite("/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/VisionOut5.jpg",img);
cv::waitKey(0);
std::cout << "DONE!\n" << std::endl;
return 0;
}
