EC.cpp

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#include "EC.h"
#include "Optimization.h"

namespace alvar {

struct ProjectParams
{
        Camera *camera;
        const CvMat *object_model;
};

static void ProjectRot(CvMat* state, CvMat* projection, void* x) 
{
        ProjectParams *foo = (ProjectParams*)x;
        Camera *camera = foo->camera;
        const CvMat *object_model = foo->object_model;
        int count = projection->rows;
        CvMat rot_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+0]));
        double zeros[3] = {0};
        CvMat zero_tra = cvMat(3, 1, CV_64F, zeros);
        cvReshape(projection, projection, 2, 1);
        cvProjectPoints2(object_model, &rot_mat, &zero_tra, &(camera->calib_K), &(camera->calib_D), projection);
        cvReshape(projection, projection, 1, count);
}

// TODO: How this differs from the Camera::ProjectPoints ???
static void Project(CvMat* state, CvMat* projection, void* x)
{
        ProjectParams *foo = (ProjectParams*)x;
        Camera *camera = foo->camera;
        const CvMat *object_model = foo->object_model;
        int count = projection->rows;
        CvMat rot_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+0]));
        CvMat tra_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+3]));
        cvReshape(projection, projection, 2, 1);
        cvProjectPoints2(object_model, &rot_mat, &tra_mat, &(camera->calib_K), &(camera->calib_D), projection);
        cvReshape(projection, projection, 1, count);
}

bool CameraEC::UpdatePose(const CvMat* object_points, CvMat* image_points, Pose *pose, CvMat *weights) {
        double rot[3]; CvMat rotm = cvMat(3, 1, CV_64F, rot);
        double tra[3]; CvMat tram = cvMat(3, 1, CV_64F, tra);
        pose->GetRodriques(&rotm);
        pose->GetTranslation(&tram);
        bool ret = UpdatePose(object_points, image_points, &rotm, &tram, weights);
        pose->SetRodriques(&rotm);
        pose->SetTranslation(&tram);
        return ret;
}

bool CameraEC::UpdatePose(const CvMat* object_points, CvMat* image_points, CvMat *rot, CvMat *tra, CvMat *weights) {
        if (object_points->height < 4) return false;
        /*      if (object_points->height < 6) {
                return false;
                // TODO: We need to change image_points into CV_32FC2
                return Camera::CalcExteriorOrientation(object_points, image_points, rot, tra);
        }*/
        CvMat* par = cvCreateMat(6, 1, CV_64F);
        memcpy(&(par->data.db[0+0]), rot->data.db, 3*sizeof(double));
        memcpy(&(par->data.db[0+3]), tra->data.db, 3*sizeof(double));

        ProjectParams pparams;
        pparams.camera = this;
        pparams.object_model  = object_points;

        alvar::Optimization *opt = new alvar::Optimization(6, image_points->height);
        double tmp = opt->Optimize(par, image_points, 0.0005, 2, Project, &pparams, alvar::Optimization::TUKEY_LM, 0, 0, weights);

        memcpy(rot->data.db, &(par->data.db[0+0]), 3*sizeof(double));
        memcpy(tra->data.db, &(par->data.db[0+3]), 3*sizeof(double));
        
        delete opt;

        cvReleaseMat(&par);
        return true;
}

bool CameraEC::UpdateRotation(const CvMat* object_points, CvMat* image_points, Pose *pose)
{
        double rot[3]; CvMat rotm = cvMat(3, 1, CV_64F, rot);
        double tra[3]; CvMat tram = cvMat(3, 1, CV_64F, tra);
        pose->GetRodriques(&rotm);
        pose->GetTranslation(&tram);
        bool ret = UpdateRotation(object_points, image_points, &rotm, &tram);
        pose->SetRodriques(&rotm);
        pose->SetTranslation(&tram);
        return ret;     
}

bool CameraEC::UpdateRotation(const CvMat* object_points, CvMat* image_points, CvMat *rot, CvMat *tra) {

        CvMat* par = cvCreateMat(3, 1, CV_64F);
        memcpy(&(par->data.db[0+0]), rot->data.db, 3*sizeof(double));
        ProjectParams pparams;
        pparams.camera = this;
        pparams.object_model  = object_points;
        alvar::Optimization *opt = new alvar::Optimization(3, image_points->height);
        double tmp = opt->Optimize(par, image_points, 0.0005, 2, ProjectRot, &pparams, alvar::Optimization::TUKEY_LM);
        memcpy(rot->data.db, &(par->data.db[0+0]), 3*sizeof(double));
        delete opt;
        cvReleaseMat(&par);
        return true;
}

// Ol etta mirror asia on kunnossa
void GetOrigo(Pose* pose, CvMat* O)
{
        pose->GetTranslation(O);
}

void GetPointOnLine(const Pose* pose, Camera *camera, const CvPoint2D32f *u, CvMat* P)
{
        double kid[9], rotd[9], trad[3], ud[3] = {u->x, u->y, 1};
        CvMat Ki = cvMat(3, 3, CV_64F, kid);
        CvMat R = cvMat(3, 3, CV_64F, rotd);
        CvMat T = cvMat(3, 1, CV_64F, trad);
        CvMat U = cvMat(3, 1, CV_64F, ud);
        pose->GetMatrix(&R);
        pose->GetTranslation(&T);
        cvInv(&(camera->calib_K), &Ki);
        cvMatMul(&R, &Ki, &Ki);
        cvGEMM(&Ki, &U, 1, &T, 1, P, 0);                                        
}

bool MidPointAlgorithm(CvMat* o1, CvMat* o2, CvMat* p1, CvMat* p2, CvPoint3D32f& X, double limit)
{
        double ud[3], vd[3], wd[3];
        CvMat u = cvMat(3, 1, CV_64F, ud);
        CvMat v = cvMat(3, 1, CV_64F, vd);
        CvMat w = cvMat(3, 1, CV_64F, wd);
        
        cvSub(p1, o1, &u);
        cvSub(p2, o2, &v);
        cvSub(o1, o2, &w);

        double a = cvDotProduct(&u, &u);
        double b = cvDotProduct(&u, &v);
        double c = cvDotProduct(&v, &v);
        double d = cvDotProduct(&u, &w);
        double e = cvDotProduct(&v, &w);
        double D = a*c - b*b;
        double sc, tc;

        // compute the line parameters of the two closest points
        if (D < limit)
        {   
                return false;
                // the lines are almost parallel
                sc = 0.0;
                tc = (b>c ? d/b : e/c);   // use the largest denominator
        }
        else
        {
                sc = (b*e - c*d) / D;
                tc = (a*e - b*d) / D;
        }
        
        double m1d[3], m2d[3];
        CvMat m1 = cvMat(3, 1, CV_64F, m1d);
        CvMat m2 = cvMat(3, 1, CV_64F, m2d);
        cvAddWeighted(&u, sc, o1, 1.0, 0.0, &m1);
        cvAddWeighted(&v, tc, o2, 1.0, 0.0, &m2);
        cvAddWeighted(&m1, 0.5, &m2, 0.5, 0.0, &m1);    
        
        X.x = (float)m1d[0];
        X.y = (float)m1d[1];
        X.z = (float)m1d[2];

        return true;
}

// todo
bool CameraEC::ReconstructFeature(const Pose *pose1, const Pose *pose2, const CvPoint2D32f *u1, const CvPoint2D32f *u2, CvPoint3D32f *p3d, double limit) {
        double o1d[3], o2d[3], p1d[3], p2d[3];
        CvMat o1 = cvMat(3, 1, CV_64F, o1d);
        CvMat o2 = cvMat(3, 1, CV_64F, o2d);
        CvMat p1 = cvMat(3, 1, CV_64F, p1d);
        CvMat p2 = cvMat(3, 1, CV_64F, p2d);

        Pose po1 = *pose1; // Make copy so that we don't destroy the pose content
        Pose po2 = *pose2;
        po1.Invert();
        po2.Invert();
        GetOrigo(&po1, &o1);
        GetOrigo(&po2, &o2);
        GetPointOnLine(&po1, this, u1, &p1);
        GetPointOnLine(&po2, this, u2, &p2);
        
        return MidPointAlgorithm(&o1, &o2, &p1, &p2, *p3d, limit);
}

void CameraEC::Get3dOnPlane(const Pose *pose, CvPoint2D32f p2d, CvPoint3D32f &p3d) {
        double pd[16], md[9], kd[9];    
        CvMat P = cvMat(4, 4, CV_64F, pd);
        CvMat H = cvMat(3, 3, CV_64F, md);
        CvMat Ki = cvMat(3, 3, CV_64F, kd);
        
        pose->GetMatrix(&P);
        cvInv(&(calib_K), &Ki);
        
        // Construct homography from pose
        int ind_s = 0, ind_c = 0;
        for(int i = 0; i < 3; ++i)
        {
                CvRect r; r.x = ind_s; r.y = 0; r.height = 3; r.width = 1;
                CvMat sub = cvMat(3, 1, CV_64F);
                cvGetSubRect(&P, &sub, r);
                CvMat col = cvMat(3, 1, CV_64F);
                cvGetCol(&H, &col, ind_c);
                cvCopy(&sub, &col);
                ind_c++;
                ind_s++;
                if(i == 1) ind_s++; 
        }

        // Apply H to get the 3D coordinates
        Camera::Undistort(p2d);
        double xd[3] = {p2d.x, p2d.y, 1};
        CvMat X = cvMat(3, 1, CV_64F, xd);
        cvMatMul(&Ki, &X, &X);
        cvInv(&H, &H);
        cvMatMul(&H, &X, &X);

        p3d.x = (float)(xd[0] / xd[2]);
        p3d.y = (float)(xd[1] / xd[2]);
        p3d.z = 0;
}

void CameraEC::Get3dOnDepth(const Pose *pose, CvPoint2D32f p2d, float depth, CvPoint3D32f &p3d)
{
        double wx, wy, wz;
        Camera::Undistort(p2d);
        
        // Tassa asetetaan z = inf ja lasketaan x ja y jotenkin?!?
        //double object_scale = _dist_to_target; // TODO Same as the pose?!?!?!?
                                        
        // inv(K)*[u v 1]'*scale
        wx =  depth*(p2d.x-calib_K_data[0][2])/calib_K_data[0][0];
        wy =  depth*(p2d.y-calib_K_data[1][2])/calib_K_data[1][1];
        wz =  depth;

        // Now the points are in camera coordinate frame.
        alvar::Pose p = *pose;
        p.Invert();
        
        double Xd[4] = {wx, wy, wz, 1};
        CvMat Xdm = cvMat(4, 1, CV_64F, Xd);
        double Pd[16];
        CvMat Pdm = cvMat(4, 4, CV_64F, Pd);
        p.GetMatrix(&Pdm);
        cvMatMul(&Pdm, &Xdm, &Xdm);
        p3d.x = float(Xd[0]/Xd[3]);
        p3d.y = float(Xd[1]/Xd[3]);
        p3d.z = float(Xd[2]/Xd[3]);
}

} // namespace alvar