Robotics with ROS

Kalman Filter Explained With Python Code From Scratch

23 Comments / C++, Machine Learning, Python, Robotic, Tutorials / admin

This snippet shows tracking mouse cursor with Python code from scratch and comparing the result with OpenCV. The CSV file that has been used are being created with below c++ code. A sample could be downloaded from here 1, 2, 3. Python Kalman Filter

import numpy as np
np.set_printoptions(threshold=3)
np.set_printoptions(suppress=True)
from numpy import genfromtxt


#Notation used coming from: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/


def prediction(X_hat_t_1,P_t_1,F_t,B_t,U_t,Q_t):
    X_hat_t=F_t.dot(X_hat_t_1)+(B_t.dot(U_t).reshape(B_t.shape[0],-1) )
    P_t=np.diag(np.diag(F_t.dot(P_t_1).dot(F_t.transpose())))+Q_t
    return X_hat_t,P_t
    

def update(X_hat_t,P_t,Z_t,R_t,H_t):
    
    K_prime=P_t.dot(H_t.transpose()).dot( np.linalg.inv ( H_t.dot(P_t).dot(H_t.transpose()) +R_t ) )  
    print("K:\n",K_prime)
    
    X_t=X_hat_t+K_prime.dot(Z_t-H_t.dot(X_hat_t))
    P_t=P_t-K_prime.dot(H_t).dot(P_t)
    
    return X_t,P_t


acceleration=0
delta_t=1/20#milisecond

groundTruth = genfromtxt('data/groundTruth.csv', delimiter=',',skip_header=1)

#Observations: position_X, position_Y
measurmens = genfromtxt('data/measurmens.csv', delimiter=',',skip_header=1)


#Checking our result with OpenCV
opencvKalmanOutput = genfromtxt('data/kalmanv.csv', delimiter=',',skip_header=1)

#Transition matrix
F_t=np.array([ [1 ,0,delta_t,0] , [0,1,0,delta_t] , [0,0,1,0] , [0,0,0,1] ])

#Initial State cov
P_t= np.identity(4)*0.2

#Process cov
Q_t= np.identity(4)

#Control matrix
B_t=np.array( [ [0] , [0], [0] , [0] ])

#Control vector
U_t=acceleration

#Measurment Matrix
H_t = np.array([ [1, 0, 0, 0], [ 0, 1, 0, 0]])

#Measurment cov
R_t= np.identity(2)*5

# Initial State
X_hat_t = np.array( [[0],[0],[0],[0]] )
print("X_hat_t",X_hat_t.shape)
print("P_t",P_t.shape)
print("F_t",F_t.shape)
print("B_t",B_t.shape)
print("Q_t",Q_t.shape)
print("R_t",R_t.shape)
print("H_t",H_t.shape)

for i in range(measurmens.shape[0]):
    X_hat_t,P_hat_t = prediction(X_hat_t,P_t,F_t,B_t,U_t,Q_t)
    print("Prediction:")
    print("X_hat_t:\n",X_hat_t,"\nP_t:\n",P_t)
    
    Z_t=measurmens[i].transpose()
    Z_t=Z_t.reshape(Z_t.shape[0],-1)
    
    print(Z_t.shape)
    
    X_t,P_t=update(X_hat_t,P_hat_t,Z_t,R_t,H_t)
    print("Update:")
    print("X_t:\n",X_t,"\nP_t:\n",P_t)
    X_hat_t=X_t
    P_hat_t=P_t
    
    print("=========================================")
    print("Opencv Kalman Output:")
    print("X_t:\n",opencvKalmanOutput[i])

import numpy as np

np.set_printoptions(threshold=3)

np.set_printoptions(suppress=True)

from numpy import genfromtxt

#Notation used coming from: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

def prediction(X_hat_t_1,P_t_1,F_t,B_t,U_t,Q_t):

X_hat_t=F_t.dot(X_hat_t_1)+(B_t.dot(U_t).reshape(B_t.shape[0],-1) )

P_t=np.diag(np.diag(F_t.dot(P_t_1).dot(F_t.transpose())))+Q_t

return X_hat_t,P_t

def update(X_hat_t,P_t,Z_t,R_t,H_t):

K_prime=P_t.dot(H_t.transpose()).dot( np.linalg.inv ( H_t.dot(P_t).dot(H_t.transpose()) +R_t ) )

print("K:\n",K_prime)

X_t=X_hat_t+K_prime.dot(Z_t-H_t.dot(X_hat_t))

P_t=P_t-K_prime.dot(H_t).dot(P_t)

return X_t,P_t

acceleration=0

delta_t=1/20#milisecond

groundTruth = genfromtxt('data/groundTruth.csv', delimiter=',',skip_header=1)

#Observations: position_X, position_Y

measurmens = genfromtxt('data/measurmens.csv', delimiter=',',skip_header=1)

#Checking our result with OpenCV

opencvKalmanOutput = genfromtxt('data/kalmanv.csv', delimiter=',',skip_header=1)

#Transition matrix

F_t=np.array([ [1 ,0,delta_t,0] , [0,1,0,delta_t] , [0,0,1,0] , [0,0,0,1] ])

#Initial State cov

P_t= np.identity(4)*0.2

#Process cov

Q_t= np.identity(4)

#Control matrix

B_t=np.array( [ [0] , [0], [0] , [0] ])

#Control vector

U_t=acceleration

#Measurment Matrix

H_t = np.array([ [1, 0, 0, 0], [ 0, 1, 0, 0]])

#Measurment cov

R_t= np.identity(2)*5

# Initial State

X_hat_t = np.array( [[0],[0],[0],[0]] )

print("X_hat_t",X_hat_t.shape)

print("P_t",P_t.shape)

print("F_t",F_t.shape)

print("B_t",B_t.shape)

print("Q_t",Q_t.shape)

print("R_t",R_t.shape)

print("H_t",H_t.shape)

for i in range(measurmens.shape[0]):

X_hat_t,P_hat_t = prediction(X_hat_t,P_t,F_t,B_t,U_t,Q_t)

print("Prediction:")

print("X_hat_t:\n",X_hat_t,"\nP_t:\n",P_t)

Z_t=measurmens[i].transpose()

Z_t=Z_t.reshape(Z_t.shape[0],-1)

print(Z_t.shape)

X_t,P_t=update(X_hat_t,P_hat_t,Z_t,R_t,H_t)

print("Update:")

print("X_t:\n",X_t,"\nP_t:\n",P_t)

X_hat_t=X_t

P_hat_t=P_t

print("=========================================")

print("Opencv Kalman Output:")

print("X_t:\n",opencvKalmanOutput[i])

C++ and OpenCV Kalman Filter Rapidcsv has been downloaded from […]

Kalman Filter Explained With Python Code From Scratch Read More »

Eigen unaryExpr (Function Pointer, Lambda Expression) Example

Leave a Comment / C++, Linear Algebra, Tutorials / admin

double ramp(double x)
{
    if (x > 0)
        return x;
    else
        return 0;
}

void unaryExprExample()
{
    //unaryExpr is a const function so it will not give you the possibility to modify values in place
    Eigen::ArrayXd x = Eigen::ArrayXd::Random(5);
    std::cout<<x <<std::endl;
    x = x.unaryExpr([](double elem) // changed type of parameter
    {
        return elem < 0.0 ? 0.0 : 1.0; // return instead of assignment
    });
    std::cout<<x <<std::endl;
    std::cout << x.unaryExpr(std::ptr_fun(ramp)) << std::endl;

}

double ramp(double x)

{

if (x > 0)

return x;

else

return 0;

}

void unaryExprExample()

{

//unaryExpr is a const function so it will not give you the possibility to modify values in place

Eigen::ArrayXd x = Eigen::ArrayXd::Random(5);

std::cout<<x <<std::endl;

x = x.unaryExpr([](double elem) // changed type of parameter

{

return elem < 0.0 ? 0.0 : 1.0; // return instead of assignment

});

std::cout<<x <<std::endl;

std::cout << x.unaryExpr(std::ptr_fun(ramp)) << std::endl;

}

Eigen unaryExpr (Function Pointer, Lambda Expression) Example Read More »

Matrix Decomposition with Eigen: QR, Cholesky Decomposition LU, UL

Leave a Comment / C++, Linear Algebra, Tutorials / admin

void qRDecomposition(Eigen::MatrixXd &A,Eigen::MatrixXd &Q, Eigen::MatrixXd &R)
{
    /*
        A=QR
        Q: is orthogonal matrix-> columns of Q are orthonormal
        R: is upper triangulate matrix

        this is possible when columns of A are linearly indipendent

    */

    Eigen::MatrixXd thinQ(A.rows(),A.cols() ), q(A.rows(),A.rows());

    Eigen::HouseholderQR<Eigen::MatrixXd> householderQR(A);
    q = householderQR.householderQ();
    thinQ.setIdentity();
    Q = householderQR.householderQ() * thinQ;
    R=Q.transpose()*A;
}

void qRExample()
{

    Eigen::MatrixXd A;
    A.setRandom(3,4);

    std::cout<<"A" <<std::endl;
    std::cout<<A <<std::endl;
    Eigen::MatrixXd Q(A.rows(),A.rows());
    Eigen::MatrixXd R(A.rows(),A.cols());

    /////////////////////////////////HouseholderQR////////////////////////
    Eigen::MatrixXd thinQ(A.rows(),A.cols() ), q(A.rows(),A.rows());

    Eigen::HouseholderQR<Eigen::MatrixXd> householderQR(A);
    q = householderQR.householderQ();
    thinQ.setIdentity();
    Q = householderQR.householderQ() * thinQ;

    std::cout << "HouseholderQR" <<std::endl;

    std::cout << "Q" <<std::endl;
    std::cout << Q <<std::endl;

    R = householderQR.matrixQR().template  triangularView<Eigen::Upper>();
    std::cout << R<<std::endl;
    std::cout << R.rows()<<std::endl;
    std::cout << R.cols()<<std::endl;


    R=Q.transpose()*A;
// 	std::cout << "R" <<std::endl;
// 	std::cout << R<<std::endl;

    std::cout << "A-Q*R" <<std::endl;
    std::cout << A-Q*R <<std::endl;

    /////////////////////////////////ColPivHouseholderQR////////////////////////
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> colPivHouseholderQR(A.rows(), A.cols());
    colPivHouseholderQR.compute(A);
    //R = colPivHouseholderQR.matrixR().template triangularView<Upper>();
    R = colPivHouseholderQR.matrixR();
    Q = colPivHouseholderQR.matrixQ();

    std::cout << "ColPivHouseholderQR" <<std::endl;

    std::cout << "Q" <<std::endl;
    std::cout << Q <<std::endl;

    std::cout << "R" <<std::endl;
    std::cout << R <<std::endl;

    std::cout << "A-Q*R" <<std::endl;
    std::cout << A-Q*R <<std::endl;

    /////////////////////////////////FullPivHouseholderQR////////////////////////
    std::cout << "FullPivHouseholderQR" <<std::endl;

    Eigen::FullPivHouseholderQR<Eigen::MatrixXd> fullPivHouseholderQR(A.rows(), A.cols());
    fullPivHouseholderQR.compute(A);
    Q=fullPivHouseholderQR.matrixQ();
    R=fullPivHouseholderQR.matrixQR().template  triangularView<Eigen::Upper>();

    std::cout << "Q" <<std::endl;
    std::cout << Q <<std::endl;

    std::cout << "R" <<std::endl;
    std::cout << R <<std::endl;

    std::cout << "A-Q*R" <<std::endl;
    std::cout << A-Q*R <<std::endl;

}

void lDUDecomposition()
{
/*

    L: lower triangular matrix L
    U: upper triangular matrix U
    D: is a diagonal matrix
    A=LDU


*/
}

void lUDecomposition()
{
/*
    L: lower triangular matrix L
    U: upper triangular matrix U
    A=LU
*/
}

double exp(double x) // the functor we want to apply
{
    std::setprecision(5);
        return std::trunc(x);
}

void gramSchmidtOrthogonalization(Eigen::MatrixXd &matrix,Eigen::MatrixXd &orthonormalMatrix)
{
/*
    In this method you make every column perpendicular to it's previous columns,
    here if a and b are representation vector of two columns, c=b-((b.a)/|a|).a
        ^
       /
    b /
     /
    /
    ---------->
        a

        ^
       /|
    b / |
     /  | c
    /   |
    ---------->
        a

    you just have to normilze every vector after make it perpendicular to previous columns
    so:
    q1=a.normalized();
    q2=b-(b.q1).q1
    q2=q2.normalized();
    q3=c-(c.q1).q1 - (c.q2).q2
    q3=q3.normalized();


    Now we have Q, but we want A=QR so we just multiply both side by Q.transpose(), since Q is orthonormal, Q*Q.transpose() is I
    A=QR;
    Q.transpose()*A=R;
*/
    Eigen::VectorXd col;
    for(int i=0;i<matrix.cols();i++)
    {
        col=matrix.col(i);
        col=col.normalized();
        for(int j=0;j<i-1;j++)
        {
            //orthonormalMatrix.col(i)
        }

        orthonormalMatrix.col(i)=col;
    }
    Eigen::MatrixXd A(4,3);

    A<<1,2,3,-1,1,1,1,1,1,1,1,1;
    Eigen::Vector4d a=A.col(0);
    Eigen::Vector4d b=A.col(1);
    Eigen::Vector4d c=A.col(2);

    Eigen::Vector4d q1=  a.normalized();
    Eigen::Vector4d q2=b-(b.dot(q1))*q1;
    q2=q2.normalized();

    Eigen::Vector4d q3=c-(c.dot(q1))*q1 - (c.dot(q2))*q2;
    q3=q3.normalized();

    std::cout<< "q1:"<<std::endl;
    std::cout<< q1<<std::endl;
    std::cout<< "q2"<<std::endl;
    std::cout<< q2<<std::endl;
    std::cout<< "q3:"<<std::endl;
    std::cout<< q3<<std::endl;

    Eigen::MatrixXd Q(4,3);
    Q.col(0)=q1;
    Q.col(1)=q2;
    Q.col(2)=q3;

    Eigen::MatrixXd R(3,3);
    R=Q.transpose()*(A);


    std::cout<<"Q"<<std::endl;
    std::cout<< Q<<std::endl;


    std::cout<<"R"<<std::endl;
    std::cout<< R.unaryExpr(std::ptr_fun(exp))<<std::endl;



    //MatrixXd A(4,3), thinQ(4,3), Q(4,4);

    Eigen::MatrixXd thinQ(4,3), q(4,4);

    //A.setRandom();
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    q = qr.householderQ();
    thinQ.setIdentity();
    thinQ = qr.householderQ() * thinQ;
    std::cout << "Q computed by Eigen" << "\n\n" << thinQ << "\n\n";
    std::cout << q << "\n\n" << thinQ << "\n\n";


}

void gramSchmidtOrthogonalizationExample()
{
    Eigen::MatrixXd matrix(3,4),orthonormalMatrix(3,4) ;
    matrix=Eigen::MatrixXd::Random(3,4);////A.setRandom();


    gramSchmidtOrthogonalization(matrix,orthonormalMatrix);
}

void choleskyDecompositionExample()
{
/*
Positive-definite matrix:
    Matrix Mnxn is said to be positive definite if the scalar zTMz is strictly positive for every non-zero
    column vector z n real numbers. zTMz>0


Hermitian matrix:
    Matrix Mnxn is said to be positive definite if the scalar z*Mz is strictly positive for every non-zero
    column vector z n real numbers. z*Mz>0
    z* is the conjugate transpose of z.

Positive semi-definite same as above except zTMz>=0 or  z*Mz>=0


    Example:
            ┌2  -1  0┐
        M=	|-1  2 -1|
            |0 - 1  2|
            └        ┘
          ┌ a ┐
        z=| b |
          | c |
          └	  ┘
        zTMz=a^2 +c^2+ (a-b)^2+ (b-c)^2

Cholesky decomposition:
Cholesky decomposition of a Hermitian positive-definite matrix A is:
    A=LL*

    L is a lower triangular matrix with real and positive diagonal entries
    L* is the conjugate transpose of L
*/

    Eigen::MatrixXd A(3,3);
    A << 6, 0, 0, 0, 4, 0, 0, 0, 7;
    Eigen::MatrixXd L( A.llt().matrixL() );
    Eigen::MatrixXd L_T=L.adjoint();//conjugate transpose

    std::cout << "L" << std::endl;
    std::cout << L << std::endl;
    std::cout << "L_T" << std::endl;
    std::cout << L_T << std::endl;
    std::cout << "A" << std::endl;
    std::cout << A << std::endl;
    std::cout << "L*L_T" << std::endl;
    std::cout << L*L_T << std::endl;

}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

void qRDecomposition(Eigen::MatrixXd &A,Eigen::MatrixXd &Q, Eigen::MatrixXd &R)

{

A=QR

Q: is orthogonal matrix-> columns of Q are orthonormal

R: is upper triangulate matrix

this is possible when columns of A are linearly indipendent

Eigen::MatrixXd thinQ(A.rows(),A.cols() ), q(A.rows(),A.rows());

Eigen::HouseholderQR<Eigen::MatrixXd> householderQR(A);

q = householderQR.householderQ();

thinQ.setIdentity();

Q = householderQR.householderQ() * thinQ;

R=Q.transpose()*A;

}

void qRExample()

{

Eigen::MatrixXd A;

A.setRandom(3,4);

std::cout<<"A" <<std::endl;

std::cout<<A <<std::endl;

Eigen::MatrixXd Q(A.rows(),A.rows());

Eigen::MatrixXd R(A.rows(),A.cols());

/////////////////////////////////HouseholderQR////////////////////////

Eigen::MatrixXd thinQ(A.rows(),A.cols() ), q(A.rows(),A.rows());

Eigen::HouseholderQR<Eigen::MatrixXd> householderQR(A);

q = householderQR.householderQ();

thinQ.setIdentity();

Q = householderQR.householderQ() * thinQ;

std::cout << "HouseholderQR" <<std::endl;

std::cout << "Q" <<std::endl;

std::cout << Q <<std::endl;

R = householderQR.matrixQR().template triangularView<Eigen::Upper>();

std::cout << R<<std::endl;

std::cout << R.rows()<<std::endl;

std::cout << R.cols()<<std::endl;

R=Q.transpose()*A;

// std::cout << "R" <<std::endl;

// std::cout << R<<std::endl;

std::cout << "A-Q*R" <<std::endl;

std::cout << A-Q*R <<std::endl;

/////////////////////////////////ColPivHouseholderQR////////////////////////

Eigen::ColPivHouseholderQR<Eigen::MatrixXd> colPivHouseholderQR(A.rows(), A.cols());

colPivHouseholderQR.compute(A);

//R = colPivHouseholderQR.matrixR().template triangularView<Upper>();

R = colPivHouseholderQR.matrixR();

Q = colPivHouseholderQR.matrixQ();

std::cout << "ColPivHouseholderQR" <<std::endl;

std::cout << "Q" <<std::endl;

std::cout << Q <<std::endl;

std::cout << "R" <<std::endl;

std::cout << R <<std::endl;

std::cout << "A-Q*R" <<std::endl;

std::cout << A-Q*R <<std::endl;

/////////////////////////////////FullPivHouseholderQR////////////////////////

std::cout << "FullPivHouseholderQR" <<std::endl;

Eigen::FullPivHouseholderQR<Eigen::MatrixXd> fullPivHouseholderQR(A.rows(), A.cols());

fullPivHouseholderQR.compute(A);

Q=fullPivHouseholderQR.matrixQ();

R=fullPivHouseholderQR.matrixQR().template triangularView<Eigen::Upper>();

std::cout << "Q" <<std::endl;

std::cout << Q <<std::endl;

std::cout << "R" <<std::endl;

std::cout << R <<std::endl;

std::cout << "A-Q*R" <<std::endl;

std::cout << A-Q*R <<std::endl;

}

void lDUDecomposition()

{

L: lower triangular matrix L

U: upper triangular matrix U

D: is a diagonal matrix

A=LDU

}

void lUDecomposition()

{

L: lower triangular matrix L

U: upper triangular matrix U

A=LU

}

double exp(double x) // the functor we want to apply

{

std::setprecision(5);

return std::trunc(x);

}

void gramSchmidtOrthogonalization(Eigen::MatrixXd &matrix,Eigen::MatrixXd &orthonormalMatrix)

{

In this method you make every column perpendicular to it's previous columns,

here if a and b are representation vector of two columns, c=b-((b.a)/|a|).a

b /

---------->

b / |

/ | c

/ |

---------->

you just have to normilze every vector after make it perpendicular to previous columns

so:

q1=a.normalized();

q2=b-(b.q1).q1

q2=q2.normalized();

q3=c-(c.q1).q1 - (c.q2).q2

q3=q3.normalized();

Now we have Q, but we want A=QR so we just multiply both side by Q.transpose(), since Q is orthonormal, Q*Q.transpose() is I

A=QR;

Q.transpose()*A=R;

Eigen::VectorXd col;

for(int i=0;i<matrix.cols();i++)

{

col=matrix.col(i);

col=col.normalized();

for(int j=0;j<i-1;j++)

{

//orthonormalMatrix.col(i)

}

orthonormalMatrix.col(i)=col;

}

Eigen::MatrixXd A(4,3);

A<<1,2,3,-1,1,1,1,1,1,1,1,1;

Eigen::Vector4d a=A.col(0);

Eigen::Vector4d b=A.col(1);

Eigen::Vector4d c=A.col(2);

Eigen::Vector4d q1= a.normalized();

Eigen::Vector4d q2=b-(b.dot(q1))*q1;

q2=q2.normalized();

Eigen::Vector4d q3=c-(c.dot(q1))*q1 - (c.dot(q2))*q2;

q3=q3.normalized();

std::cout<< "q1:"<<std::endl;

std::cout<< q1<<std::endl;

std::cout<< "q2"<<std::endl;

std::cout<< q2<<std::endl;

std::cout<< "q3:"<<std::endl;

std::cout<< q3<<std::endl;

Eigen::MatrixXd Q(4,3);

Q.col(0)=q1;

Q.col(1)=q2;

Q.col(2)=q3;

Eigen::MatrixXd R(3,3);

R=Q.transpose()*(A);

std::cout<<"Q"<<std::endl;

std::cout<< Q<<std::endl;

std::cout<<"R"<<std::endl;

std::cout<< R.unaryExpr(std::ptr_fun(exp))<<std::endl;

//MatrixXd A(4,3), thinQ(4,3), Q(4,4);

Eigen::MatrixXd thinQ(4,3), q(4,4);

//A.setRandom();

Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);

q = qr.householderQ();

thinQ.setIdentity();

thinQ = qr.householderQ() * thinQ;

std::cout << "Q computed by Eigen" << "\n\n" << thinQ << "\n\n";

std::cout << q << "\n\n" << thinQ << "\n\n";

}

void gramSchmidtOrthogonalizationExample()

{

Eigen::MatrixXd matrix(3,4),orthonormalMatrix(3,4) ;

matrix=Eigen::MatrixXd::Random(3,4);////A.setRandom();

gramSchmidtOrthogonalization(matrix,orthonormalMatrix);

}

void choleskyDecompositionExample()

{

Positive-definite matrix:

Matrix Mnxn is said to be positive definite if the scalar zTMz is strictly positive for every non-zero

column vector z n real numbers. zTMz>0

Hermitian matrix:

Matrix Mnxn is said to be positive definite if the scalar z*Mz is strictly positive for every non-zero

column vector z n real numbers. z*Mz>0

z* is the conjugate transpose of z.

Positive semi-definite same as above except zTMz>=0 or z*Mz>=0

Example:

┌2 -1 0┐

M= |-1 2 -1|

|0 - 1 2|

└ ┘

┌ a ┐

z=| b |

| c |

└ ┘

zTMz=a^2 +c^2+ (a-b)^2+ (b-c)^2

Cholesky decomposition:

Cholesky decomposition of a Hermitian positive-definite matrix A is:

A=LL*

L is a lower triangular matrix with real and positive diagonal entries

L* is the conjugate transpose of L

Eigen::MatrixXd A(3,3);

A << 6, 0, 0, 0, 4, 0, 0, 0, 7;

Eigen::MatrixXd L( A.llt().matrixL() );

Eigen::MatrixXd L_T=L.adjoint();//conjugate transpose

std::cout << "L" << std::endl;

std::cout << L << std::endl;

std::cout << "L_T" << std::endl;

std::cout << L_T << std::endl;

std::cout << "A" << std::endl;

std::cout << A << std::endl;

std::cout << "L*L_T" << std::endl;

std::cout << L*L_T << std::endl;

}

Matrix Decomposition with Eigen: QR, Cholesky Decomposition LU, UL Read More »

Eigen Memory Mapping

Leave a Comment / C++, Tutorials / admin

struct point {
    double a;
    double b;
};

void EigenMapExample()
{
    ////////////////////////////////////////First Example/////////////////////////////////////////
    Eigen::VectorXd solutionVec(12,1);
    solutionVec<<1,2,3,4,5,6,7,8,9,10,11,12;
    Eigen::Map<Eigen::MatrixXd> solutionColMajor(solutionVec.data(),4,3);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> >solutionRowMajor (solutionVec.data());


    std::cout << "solutionColMajor: "<< std::endl;
    std::cout << solutionColMajor<< std::endl;

    std::cout << "solutionRowMajor"<< std::endl;
    std::cout << solutionRowMajor<< std::endl;

    ////////////////////////////////////////Second Example/////////////////////////////////////////

    // https://stackoverflow.com/questions/49813340/stdvectoreigenvector3d-to-eigenmatrixxd-eigen

    int array[9];
    for (int i = 0; i < 9; ++i) {
        array[i] = i;
    }

    Eigen::MatrixXi a(9, 1);
    a = Eigen::Map<Eigen::Matrix3i>(array);
    std::cout << a << std::endl;

    std::vector<point> pointsVec;
    point point1, point2, point3;

    point1.a = 1.0;
    point1.b = 1.5;

    point2.a = 2.4;
    point2.b = 3.5;

    point3.a = -1.3;
    point3.b = 2.4;

    pointsVec.push_back(point1);
    pointsVec.push_back(point2);
    pointsVec.push_back(point3);

    Eigen::Matrix2Xd pointsMatrix2d = Eigen::Map<Eigen::Matrix2Xd>(
        reinterpret_cast<double*>(pointsVec.data()), 2,  long(pointsVec.size()));

    Eigen::MatrixXd pointsMatrixXd = Eigen::Map<Eigen::MatrixXd>(
        reinterpret_cast<double*>(pointsVec.data()), 2, long(pointsVec.size()));

    std::cout << pointsMatrix2d << std::endl;
    std::cout << "==============================" << std::endl;
    std::cout << pointsMatrixXd << std::endl;
    std::cout << "==============================" << std::endl;

    std::vector<Eigen::Vector3d> eigenPointsVec;
    eigenPointsVec.push_back(Eigen::Vector3d(2, 4, 1));
    eigenPointsVec.push_back(Eigen::Vector3d(7, 3, 9));
    eigenPointsVec.push_back(Eigen::Vector3d(6, 1, -1));
    eigenPointsVec.push_back(Eigen::Vector3d(-6, 9, 8));

    Eigen::MatrixXd pointsMatrix = Eigen::Map<Eigen::MatrixXd>(eigenPointsVec[0].data(), 3, long(eigenPointsVec.size()));

    std::cout << pointsMatrix << std::endl;
    std::cout << "==============================" << std::endl;

    pointsMatrix = Eigen::Map<Eigen::MatrixXd>(reinterpret_cast<double*>(eigenPointsVec.data()), 3, long(eigenPointsVec.size()));

    std::cout << pointsMatrix << std::endl;

    std::vector<double> aa = { 1, 2, 3, 4 };
    Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(aa.data(), long(aa.size()));
}

struct point {

double a;

double b;

};

void EigenMapExample()

{

////////////////////////////////////////First Example/////////////////////////////////////////

Eigen::VectorXd solutionVec(12,1);

solutionVec<<1,2,3,4,5,6,7,8,9,10,11,12;

Eigen::Map<Eigen::MatrixXd> solutionColMajor(solutionVec.data(),4,3);

Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> >solutionRowMajor (solutionVec.data());

std::cout << "solutionColMajor: "<< std::endl;

std::cout << solutionColMajor<< std::endl;

std::cout << "solutionRowMajor"<< std::endl;

std::cout << solutionRowMajor<< std::endl;

////////////////////////////////////////Second Example/////////////////////////////////////////

// https://stackoverflow.com/questions/49813340/stdvectoreigenvector3d-to-eigenmatrixxd-eigen

int array[9];

for (int i = 0; i < 9; ++i) {

array[i] = i;

}

Eigen::MatrixXi a(9, 1);

a = Eigen::Map<Eigen::Matrix3i>(array);

std::cout << a << std::endl;

std::vector<point> pointsVec;

point point1, point2, point3;

point1.a = 1.0;

point1.b = 1.5;

point2.a = 2.4;

point2.b = 3.5;

point3.a = -1.3;

point3.b = 2.4;

pointsVec.push_back(point1);

pointsVec.push_back(point2);

pointsVec.push_back(point3);

Eigen::Matrix2Xd pointsMatrix2d = Eigen::Map<Eigen::Matrix2Xd>(

reinterpret_cast<double*>(pointsVec.data()), 2, long(pointsVec.size()));

Eigen::MatrixXd pointsMatrixXd = Eigen::Map<Eigen::MatrixXd>(

reinterpret_cast<double*>(pointsVec.data()), 2, long(pointsVec.size()));

std::cout << pointsMatrix2d << std::endl;

std::cout << "==============================" << std::endl;

std::cout << pointsMatrixXd << std::endl;

std::cout << "==============================" << std::endl;

std::vector<Eigen::Vector3d> eigenPointsVec;

eigenPointsVec.push_back(Eigen::Vector3d(2, 4, 1));

eigenPointsVec.push_back(Eigen::Vector3d(7, 3, 9));

eigenPointsVec.push_back(Eigen::Vector3d(6, 1, -1));

eigenPointsVec.push_back(Eigen::Vector3d(-6, 9, 8));

Eigen::MatrixXd pointsMatrix = Eigen::Map<Eigen::MatrixXd>(eigenPointsVec[0].data(), 3, long(eigenPointsVec.size()));

std::cout << pointsMatrix << std::endl;

std::cout << "==============================" << std::endl;

pointsMatrix = Eigen::Map<Eigen::MatrixXd>(reinterpret_cast<double*>(eigenPointsVec.data()), 3, long(eigenPointsVec.size()));

std::cout << pointsMatrix << std::endl;

std::vector<double> aa = { 1, 2, 3, 4 };

Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(aa.data(), long(aa.size()));

}

Eigen Memory Mapping Read More »

Eigen Arrays, Matrices and Vectors: Definition, Initialization Resizing, Populating and Coefficient Wise Operations

Leave a Comment / C++, Linear Algebra, Tutorials / admin

    std::cout <<"/////////////////Definition////////////////"<<std::endl;

/*
    Definition of vectors and matrices in Eigen comes in the following form:

    Eigen::MatrixSizeType
    Eigen::VectorSizeType
    Eigen::ArraySizeType

    Size can be 2,3,4 for fixed size square matrices or X for dynamic size
    Type can be:
    i for integer,
    f for float,
    d for double,
    c for complex,
    cf for complex float,
    cd for complex double.
*/

    // Vector3f is a fixed column vector of 3 floats:
    Eigen::Vector3f objVector3f;

    // RowVector2i is a fixed row vector of 3 integer:
    Eigen::RowVector2i objRowVector2i;

    // VectorXf is a column vector of size 10 floats:
    Eigen::VectorXf objv(10);


    Eigen::Matrix4d m; // 4x4 double

    Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex


    //a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients,
    Eigen::Matrix3f a;

    //b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all.
    Eigen::MatrixXf b;

    //A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients.
    Eigen::MatrixXf A(10, 15);

    //V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients.
    Eigen::VectorXf V(30);


    //Template style definition
    // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>  &matrix

    Eigen::Matrix<double, 2, 3> my_matrix;
    my_matrix << 1, 2, 3, 4, 5, 6;

    // ArrayXf
    Eigen::Array<float, Eigen::Dynamic, 1> a1;
    // Array3f
    Eigen::Array<float, 3, 1> a2;
    // ArrayXXd
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> a3;
    // Array33d
    Eigen::Array<double, 3, 3> a4;
    Eigen::Matrix3d matrix_from_array = a4.matrix();


    std::cout <<"///////////////////Initialization//////////////////"<< std::endl;

    Eigen::Matrix2d a_2d;
    a_2d.setRandom();

    a_2d.setConstant(4.3);

    Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6);

    Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3);

    Eigen::ArrayXXf table(10, 4);
    table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90);


    std::cout <<"/////////////Matrix Coefficient Wise Operations///////////////////"<< std::endl;
    int i,j;
    std::cout << my_matrix << std::endl;
    std::cout << my_matrix.transpose()<< std::endl;

    std::cout<<my_matrix.minCoeff(&i, &j)<<std::endl;
    std::cout<<my_matrix.maxCoeff(&i, &j)<<std::endl;
    std::cout<<my_matrix.prod()<<std::endl;
    std::cout<<my_matrix.sum()<<std::endl;
    std::cout<<my_matrix.mean()<<std::endl;
    std::cout<<my_matrix.trace()<<std::endl;
    std::cout<<my_matrix.colwise().mean()<<std::endl;
    std::cout<<my_matrix.rowwise().maxCoeff()<<std::endl;
    std::cout<<my_matrix.lpNorm<2>()<<std::endl;
    std::cout<<my_matrix.lpNorm<Eigen::Infinity>()<<std::endl;
    std::cout<<(my_matrix.array()>0).all()<<std::endl;// if all elemnts are positive
    std::cout<<(my_matrix.array()>2).any()<<std::endl;//if any element is greater than 2
    std::cout<<(my_matrix.array()>1).count()<<std::endl;// count the number of elements greater than 1
    std::cout << my_matrix.array() - 2 << std::endl;
    std::cout << my_matrix.array().abs() << std::endl;
    std::cout << my_matrix.array().square() << std::endl;
    std::cout << my_matrix.array() * my_matrix.array() << std::endl;
    std::cout << my_matrix.array().exp() << std::endl;
    std::cout << my_matrix.array().log() << std::endl;
    std::cout << my_matrix.array().sqrt() << std::endl;

    std::cout <<"//////////////////Block Elements Access////////////////////"<< std::endl;
    //Block of size (p,q), starting at (i,j)	matrix.block(i,j,p,q)
    Eigen::MatrixXf mat(4, 4);
    mat << 1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16;
    std::cout << "Block in the middle" << std::endl;
    std::cout << mat.block<2, 2>(1, 1) << std::endl;

    for (int i = 1; i <= 3; ++i)
    {
        std::cout << "Block of size " << i << "x" << i << std::endl;
        std::cout << mat.block(0, 0, i, i) << std::endl;
    }


    /////////////build matrix from vector, resizing matrix, dynamic size ////////////////
    Eigen::MatrixXd dynamicMatrix;
    int rows, cols;
    rows=3;
    cols=2;
    dynamicMatrix.resize(rows,cols);
    dynamicMatrix<<-1,7,3,4,5,1;

    //If you want a conservative variant of resize() which does not change the coefficients, use conservativeResize()
    dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);
    dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(1, 4, 0);

    dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);
    dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(5, -8, 6);
/*
    you should expect this:
     1  7  1  5
     3  4  4 -8
     5  1  0  6

*/
    std::cout<< dynamicMatrix<<std::endl;

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

std::cout <<"/////////////////Definition////////////////"<<std::endl;

Definition of vectors and matrices in Eigen comes in the following form:

Eigen::MatrixSizeType

Eigen::VectorSizeType

Eigen::ArraySizeType

Size can be 2,3,4 for fixed size square matrices or X for dynamic size

Type can be:

i for integer,

f for float,

d for double,

c for complex,

cf for complex float,

cd for complex double.

// Vector3f is a fixed column vector of 3 floats:

Eigen::Vector3f objVector3f;

// RowVector2i is a fixed row vector of 3 integer:

Eigen::RowVector2i objRowVector2i;

// VectorXf is a column vector of size 10 floats:

Eigen::VectorXf objv(10);

Eigen::Matrix4d m; // 4x4 double

Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex

//a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients,

Eigen::Matrix3f a;

//b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all.

Eigen::MatrixXf b;

//A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients.

Eigen::MatrixXf A(10, 15);

//V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients.

Eigen::VectorXf V(30);

//Template style definition

// Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &matrix

Eigen::Matrix<double, 2, 3> my_matrix;

my_matrix << 1, 2, 3, 4, 5, 6;

// ArrayXf

Eigen::Array<float, Eigen::Dynamic, 1> a1;

// Array3f

Eigen::Array<float, 3, 1> a2;

// ArrayXXd

Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> a3;

// Array33d

Eigen::Array<double, 3, 3> a4;

Eigen::Matrix3d matrix_from_array = a4.matrix();

std::cout <<"///////////////////Initialization//////////////////"<< std::endl;

Eigen::Matrix2d a_2d;

a_2d.setRandom();

a_2d.setConstant(4.3);

Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6);

Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3);

Eigen::ArrayXXf table(10, 4);

table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90);

std::cout <<"/////////////Matrix Coefficient Wise Operations///////////////////"<< std::endl;

int i,j;

std::cout << my_matrix << std::endl;

std::cout << my_matrix.transpose()<< std::endl;

std::cout<<my_matrix.minCoeff(&i, &j)<<std::endl;

std::cout<<my_matrix.maxCoeff(&i, &j)<<std::endl;

std::cout<<my_matrix.prod()<<std::endl;

std::cout<<my_matrix.sum()<<std::endl;

std::cout<<my_matrix.mean()<<std::endl;

std::cout<<my_matrix.trace()<<std::endl;

std::cout<<my_matrix.colwise().mean()<<std::endl;

std::cout<<my_matrix.rowwise().maxCoeff()<<std::endl;

std::cout<<my_matrix.lpNorm<2>()<<std::endl;

std::cout<<my_matrix.lpNorm<Eigen::Infinity>()<<std::endl;

std::cout<<(my_matrix.array()>0).all()<<std::endl;// if all elemnts are positive

std::cout<<(my_matrix.array()>2).any()<<std::endl;//if any element is greater than 2

std::cout<<(my_matrix.array()>1).count()<<std::endl;// count the number of elements greater than 1

std::cout << my_matrix.array() - 2 << std::endl;

std::cout << my_matrix.array().abs() << std::endl;

std::cout << my_matrix.array().square() << std::endl;

std::cout << my_matrix.array() * my_matrix.array() << std::endl;

std::cout << my_matrix.array().exp() << std::endl;

std::cout << my_matrix.array().log() << std::endl;

std::cout << my_matrix.array().sqrt() << std::endl;

std::cout <<"//////////////////Block Elements Access////////////////////"<< std::endl;

//Block of size (p,q), starting at (i,j) matrix.block(i,j,p,q)

Eigen::MatrixXf mat(4, 4);

mat << 1, 2, 3, 4,

5, 6, 7, 8,

9, 10, 11, 12,

13, 14, 15, 16;

std::cout << "Block in the middle" << std::endl;

std::cout << mat.block<2, 2>(1, 1) << std::endl;

for (int i = 1; i <= 3; ++i)

{

std::cout << "Block of size " << i << "x" << i << std::endl;

std::cout << mat.block(0, 0, i, i) << std::endl;

}

/////////////build matrix from vector, resizing matrix, dynamic size ////////////////

Eigen::MatrixXd dynamicMatrix;

int rows, cols;

rows=3;

cols=2;

dynamicMatrix.resize(rows,cols);

dynamicMatrix<<-1,7,3,4,5,1;

//If you want a conservative variant of resize() which does not change the coefficients, use conservativeResize()

dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);

dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(1, 4, 0);

dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);

dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(5, -8, 6);

you should expect this:

1 7 1 5

3 4 4 -8

5 1 0 6

std::cout<< dynamicMatrix<<std::endl;

Eigen Arrays, Matrices and Vectors: Definition, Initialization Resizing, Populating and Coefficient Wise Operations Read More »

Computing Essential and Fundamental Matrix Using OpenCV (8 Points Algorithm) With C++

Decomposing Projection Using OpenCV and C++

Leave a Comment / C++, Computer Vision, Tutorials / admin

#include <opencv/cv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include "transformation.hpp"

//https://www.cnblogs.com/shengguang/p/5932522.html
void HouseHolderQR(const cv::Mat &A, cv::Mat &Q, cv::Mat &R)
{
    assert ( A.channels() == 1 );
    assert ( A.rows >= A.cols );
    auto sign = [](double value) { return value >= 0 ? 1: -1; };
    const auto totalRows = A.rows;
    const auto totalCols = A.cols;
    R = A.clone();
    Q = cv::Mat::eye ( totalRows, totalRows, A.type() );
    for ( int col = 0; col < A.cols; ++ col )
    {
        cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );
        cv::Mat y = matAROI.col ( 0 );
        auto yNorm = norm ( y );
        cv::Mat e1 = cv::Mat::eye ( y.rows, 1, A.type() );
        cv::Mat w = y + sign(y.at<double>(0,0)) *  yNorm * e1;
        cv::Mat v = w / norm( w );
        cv::Mat vT; cv::transpose(v, vT );
        cv::Mat I = cv::Mat::eye( matAROI.rows, matAROI.rows, A.type() );
        cv::Mat I_2VVT = I - 2 * v * vT;
        cv::Mat matH = cv::Mat::eye ( totalRows, totalRows, A.type() );
        cv::Mat matHROI = cv::Mat(matH, cv::Range ( col, totalRows ), cv::Range ( col, totalRows ) );
        I_2VVT.copyTo ( matHROI );
        R = matH * R;
        Q = Q * matH;
    }
}

int main()
{


/*
Thera are two notations for Projection Matrix

1)P=K[R|t]
                                                              ┌X┐
                                                      ┌R,t┐   |Y|
    X_Cam=R*X+t  ==> Homogeneous Coordinate ==> X_Cam=|0,1|   |Z|
                                                      └   ┘4x4└1┘4x1

                                         ┌X┐
                                ┌   ┐    |Y|
    image plane  <--x=K[I|0]3x4 |R,t|    |Z|
                                └0,1┘4x4 └1┘4x1

    P=K[R|t]

2)P=KR[I|-X0]
                                                              ┌X┐
                                                       ┌R -Rc┐|Y|
    X_Cam=R*[X_w-C]  ==> Homogeneous Coordinate ==>    |     ||Z|
                                                       └0  1 ┘└1┘
                                       ┌X┐
                                ┌R -Rc┐|Y|
    image plane  <--x=K[I|0]3x4 |0  1 ||Z|
                                └     ┘└1┘
    P=K[R|-RC]= KR[I|-C]


(1) and (2) ==> t=-RC

*/
    int numberOfPixelInHeight,numberOfPixelInWidth;
    double heightOfSensor, widthOfSensor;
    double focalLength=1.50;
    double mx, my, U0, V0;
    numberOfPixelInHeight=600;
    numberOfPixelInWidth=800;

    heightOfSensor=10;
    widthOfSensor=10;


    my=(numberOfPixelInHeight)/heightOfSensor ;
    U0=(numberOfPixelInHeight)/2 ;

    mx=(numberOfPixelInWidth)/widthOfSensor;
    V0=(numberOfPixelInWidth)/2;

    cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<
    focalLength*mx, 0, V0,
    0,focalLength*my,U0,
    0,0,1);

    double tx,ty,tz,roll,pitch,yaw;

    tx=1.0;
    ty=2.1;
    tz=-1.4;

    cv::Mat translation = (cv::Mat_<double>(3,1) <<tx,ty,tz);

    cv::Vec3d theta;

    roll=+M_PI/4 ;
    pitch=+M_PI/10;
    yaw=-+M_PI/6;

    theta[0]=roll;
    theta[1]=pitch;
    theta[2]=yaw;
    cv::Mat rotation=eulerAnglesToRotationMatrix(theta);
    //1)P=K[R|t]
    cv::Mat R_T;
    cv::hconcat(rotation, translation, R_T);
    cv::Mat projectionMatrix=cameraMatrix*R_T;



    cv::Mat calculatedCameraMatrix,calculatedRotation,calculatedTranslation;

    cv::decomposeProjectionMatrix(projectionMatrix,calculatedCameraMatrix,calculatedRotation,calculatedTranslation);


    std::cout<<"==============================Ground Truth==============================" <<std::endl;

    std::cout<<"Rotation Matrix (Ground Truth)" <<std::endl;
    std::cout<<rotation <<std::endl;

    std::cout<<"Translation Matrix (Ground Truth)" <<std::endl;
    std::cout<<translation <<std::endl;

    std::cout<<"Camera Matrix (Ground Truth)" <<std::endl;
    std::cout<<cameraMatrix <<std::endl;

    std::cout<<"==============================Decomposing Using OpenCV==============================" <<std::endl;


    std::cout<<"Computed Rotation Matrix (OpenCV)" <<std::endl;
    std::cout<<calculatedRotation <<std::endl;

    std::cout<<"Computed Translation Matrix (OpenCV)" <<std::endl;
    //std::cout<<calculatedTranslation/calculatedTranslation.at<double>(3,0) <<std::endl;
    cv::Mat tempT=(cv::Mat_<double>(3,1)<<
                   calculatedTranslation.at<double>(0,0)/calculatedTranslation.at<double>(3,0),
                   calculatedTranslation.at<double>(1,0)/calculatedTranslation.at<double>(3,0),
                   calculatedTranslation.at<double>(2,0)/calculatedTranslation.at<double>(3,0));
    std::cout<<-rotation*tempT<<std::endl;

    std::cout<<"Computed Camera Matrix (OpenCV)" <<std::endl;
    std::cout<<calculatedCameraMatrix <<std::endl;

///////////////////////////////////////decomposing the projection matrix///////////////////////////////////////////

/*

    P=KR[I|-X0]=[H_inf3x3|h3x1]
    KR=H_inf3x3
    1)X0
    -KRX0=h3x1 => X0=-(KR)^-1*h3x1 ==>X0=-(H_inf3x3)^-1*h3x1
    2)K,R

    KR=H_inf3x3 =>(KR)^-1= H_inf3x3^-1 =>R^-1*K^-1=H_inf3x3^-1 | R^-1*K^-1=Q*R => R=Q^-1, K=R^-1
                                         H_inf3x3^-1=Q*R       |

*/


    cv::Mat H_inf3x3=(cv::Mat_<double>(3,3)<< projectionMatrix.at<double>(0,0),projectionMatrix.at<double>(0,1),projectionMatrix.at<double>(0,2)
                      ,projectionMatrix.at<double>(1,0),projectionMatrix.at<double>(1,1),projectionMatrix.at<double>(1,2)
                      ,projectionMatrix.at<double>(2,0),projectionMatrix.at<double>(2,1),projectionMatrix.at<double>(2,2));

    cv::Mat h3x1=(cv::Mat_<double>(3,1)<<projectionMatrix.at<double>(0,3),projectionMatrix.at<double>(1,3),projectionMatrix.at<double>(2,3) );



    cv::Mat Q,R;
    cv::Mat H_inf3x3_inv=H_inf3x3.inv();
    //R=Q^-1, K=R^-1


    HouseHolderQR(H_inf3x3_inv, Q, R);
    cv::Mat K=R.inv();

    std::cout<<"==============================Decomposing Using My Code==============================" <<std::endl;
    //due to homogeneity we divide it by last element
    std::cout<<"Estimated Camera Matrix\n"<<K/K.at<double>(2,2) <<std::endl;

    cv::Mat rotationMatrix=Q.inv();
    std::cout<<"Estimated Camera Rotation\n"<<rotationMatrix*-1 <<std::endl;

    std::cout<<"Estimated Camera Translation" <<std::endl;

    //t=-R*C, Q.inv()=R
    std::cout<<-1*(-Q.inv()*(-H_inf3x3.inv()*h3x1 ))<<std::endl;

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

#include <opencv/cv.hpp>

#include <opencv2/core.hpp>

#include <opencv2/calib3d.hpp>

#include "transformation.hpp"

//https://www.cnblogs.com/shengguang/p/5932522.html

void HouseHolderQR(const cv::Mat &A, cv::Mat &Q, cv::Mat &R)

{

assert ( A.channels() == 1 );

assert ( A.rows >= A.cols );

auto sign = [](double value) { return value >= 0 ? 1: -1; };

const auto totalRows = A.rows;

const auto totalCols = A.cols;

R = A.clone();

Q = cv::Mat::eye ( totalRows, totalRows, A.type() );

for ( int col = 0; col < A.cols; ++ col )

{

cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );

cv::Mat y = matAROI.col ( 0 );

auto yNorm = norm ( y );

cv::Mat e1 = cv::Mat::eye ( y.rows, 1, A.type() );

cv::Mat w = y + sign(y.at<double>(0,0)) * yNorm * e1;

cv::Mat v = w / norm( w );

cv::Mat vT; cv::transpose(v, vT );

cv::Mat I = cv::Mat::eye( matAROI.rows, matAROI.rows, A.type() );

cv::Mat I_2VVT = I - 2 * v * vT;

cv::Mat matH = cv::Mat::eye ( totalRows, totalRows, A.type() );

cv::Mat matHROI = cv::Mat(matH, cv::Range ( col, totalRows ), cv::Range ( col, totalRows ) );

I_2VVT.copyTo ( matHROI );

R = matH * R;

Q = Q * matH;

}

int main()

{

Thera are two notations for Projection Matrix

1)P=K[R|t]

┌X┐

┌R,t┐ |Y|

X_Cam=R*X+t ==> Homogeneous Coordinate ==> X_Cam=|0,1| |Z|

└ ┘4x4└1┘4x1

┌X┐

┌ ┐ |Y|

image plane <--x=K[I|0]3x4 |R,t| |Z|

└0,1┘4x4 └1┘4x1

P=K[R|t]

2)P=KR[I|-X0]

┌X┐

┌R -Rc┐|Y|

X_Cam=R*[X_w-C] ==> Homogeneous Coordinate ==> | ||Z|

└0 1 ┘└1┘

┌X┐

┌R -Rc┐|Y|

image plane <--x=K[I|0]3x4 |0 1 ||Z|

└ ┘└1┘

P=K[R|-RC]= KR[I|-C]

(1) and (2) ==> t=-RC

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=1.50;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=800;

heightOfSensor=10;

widthOfSensor=10;

my=(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/widthOfSensor;

V0=(numberOfPixelInWidth)/2;

cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

double tx,ty,tz,roll,pitch,yaw;

tx=1.0;

ty=2.1;

tz=-1.4;

cv::Mat translation = (cv::Mat_<double>(3,1) <<tx,ty,tz);

cv::Vec3d theta;

roll=+M_PI/4 ;

pitch=+M_PI/10;

yaw=-+M_PI/6;

theta[0]=roll;

theta[1]=pitch;

theta[2]=yaw;

cv::Mat rotation=eulerAnglesToRotationMatrix(theta);

//1)P=K[R|t]

cv::Mat R_T;

cv::hconcat(rotation, translation, R_T);

cv::Mat projectionMatrix=cameraMatrix*R_T;

cv::Mat calculatedCameraMatrix,calculatedRotation,calculatedTranslation;

cv::decomposeProjectionMatrix(projectionMatrix,calculatedCameraMatrix,calculatedRotation,calculatedTranslation);

std::cout<<"==============================Ground Truth==============================" <<std::endl;

std::cout<<"Rotation Matrix (Ground Truth)" <<std::endl;

std::cout<<rotation <<std::endl;

std::cout<<"Translation Matrix (Ground Truth)" <<std::endl;

std::cout<<translation <<std::endl;

std::cout<<"Camera Matrix (Ground Truth)" <<std::endl;

std::cout<<cameraMatrix <<std::endl;

std::cout<<"==============================Decomposing Using OpenCV==============================" <<std::endl;

std::cout<<"Computed Rotation Matrix (OpenCV)" <<std::endl;

std::cout<<calculatedRotation <<std::endl;

std::cout<<"Computed Translation Matrix (OpenCV)" <<std::endl;

//std::cout<<calculatedTranslation/calculatedTranslation.at<double>(3,0) <<std::endl;

cv::Mat tempT=(cv::Mat_<double>(3,1)<<

calculatedTranslation.at<double>(0,0)/calculatedTranslation.at<double>(3,0),

calculatedTranslation.at<double>(1,0)/calculatedTranslation.at<double>(3,0),

calculatedTranslation.at<double>(2,0)/calculatedTranslation.at<double>(3,0));

std::cout<<-rotation*tempT<<std::endl;

std::cout<<"Computed Camera Matrix (OpenCV)" <<std::endl;

std::cout<<calculatedCameraMatrix <<std::endl;

///////////////////////////////////////decomposing the projection matrix///////////////////////////////////////////

P=KR[I|-X0]=[H_inf3x3|h3x1]

KR=H_inf3x3

1)X0

-KRX0=h3x1 => X0=-(KR)^-1*h3x1 ==>X0=-(H_inf3x3)^-1*h3x1

2)K,R

KR=H_inf3x3 =>(KR)^-1= H_inf3x3^-1 =>R^-1*K^-1=H_inf3x3^-1 | R^-1*K^-1=Q*R => R=Q^-1, K=R^-1

H_inf3x3^-1=Q*R |

cv::Mat H_inf3x3=(cv::Mat_<double>(3,3)<< projectionMatrix.at<double>(0,0),projectionMatrix.at<double>(0,1),projectionMatrix.at<double>(0,2)

,projectionMatrix.at<double>(1,0),projectionMatrix.at<double>(1,1),projectionMatrix.at<double>(1,2)

,projectionMatrix.at<double>(2,0),projectionMatrix.at<double>(2,1),projectionMatrix.at<double>(2,2));

cv::Mat h3x1=(cv::Mat_<double>(3,1)<<projectionMatrix.at<double>(0,3),projectionMatrix.at<double>(1,3),projectionMatrix.at<double>(2,3) );

cv::Mat Q,R;

cv::Mat H_inf3x3_inv=H_inf3x3.inv();

//R=Q^-1, K=R^-1

HouseHolderQR(H_inf3x3_inv, Q, R);

cv::Mat K=R.inv();

std::cout<<"==============================Decomposing Using My Code==============================" <<std::endl;

//due to homogeneity we divide it by last element

std::cout<<"Estimated Camera Matrix\n"<<K/K.at<double>(2,2) <<std::endl;

cv::Mat rotationMatrix=Q.inv();

std::cout<<"Estimated Camera Rotation\n"<<rotationMatrix*-1 <<std::endl;

std::cout<<"Estimated Camera Translation" <<std::endl;

//t=-R*C, Q.inv()=R

std::cout<<-1*(-Q.inv()*(-H_inf3x3.inv()*h3x1 ))<<std::endl;

And the output is:

==============================Ground Truth==============================
Rotation Matrix (Ground Truth)
[0.823639103546332, 0.5427868801100539, -0.1643199010764935;
 -0.4755282581475767, 0.5031184295835893, -0.7216264418079997;
 -0.3090169943749474, 0.6724985119639573, 0.6724985119639574]
Translation Matrix (Ground Truth)
[1;
 2.1;
 -1.4]
Camera Matrix (Ground Truth)
[120, 0, 400;
 0, 90, 300;
 0, 0, 1]
==============================Decomposing Using OpenCV==============================
Computed Rotation Matrix (OpenCV)
[0.823639103546332, 0.5427868801100539, -0.1643199010764936;
 -0.4755282581475769, 0.5031184295835893, -0.7216264418079997;
 -0.3090169943749474, 0.6724985119639573, 0.6724985119639574]
Computed Translation Matrix (OpenCV)
[0.9999999999999988;
 2.1;
 -1.4]
Computed Camera Matrix (OpenCV)
[120, -1.4210854715202e-14, 400;
 0, 90.00000000000001, 300;
 0, 0, 1]
==============================Decomposing Using My Code==============================
Estimated Camera Matrix
[120, -5.776629175002766e-14, 399.9999999999999;
 -5.608036291626306e-15, 89.99999999999994, 299.9999999999999;
 -1.49192016182457e-17, -1.561251128379126e-16, 1]
Estimated Camera Rotation
[0.8236391035463322, 0.5427868801100542, -0.1643199010764936;
 -0.4755282581475767, 0.5031184295835892, -0.7216264418079998;
 -0.3090169943749475, 0.6724985119639573, 0.6724985119639575]
Estimated Camera Translation
[1;
 2.099999999999999;
 -1.4]

==============================Ground Truth==============================

Rotation Matrix (Ground Truth)

[0.823639103546332, 0.5427868801100539, -0.1643199010764935;

-0.4755282581475767, 0.5031184295835893, -0.7216264418079997;

-0.3090169943749474, 0.6724985119639573, 0.6724985119639574]

Translation Matrix (Ground Truth)

[1;

2.1;

-1.4]

Camera Matrix (Ground Truth)

[120, 0, 400;

0, 90, 300;

0, 0, 1]

==============================Decomposing Using OpenCV==============================

Computed Rotation Matrix (OpenCV)

[0.823639103546332, 0.5427868801100539, -0.1643199010764936;

-0.4755282581475769, 0.5031184295835893, -0.7216264418079997;

-0.3090169943749474, 0.6724985119639573, 0.6724985119639574]

Computed Translation Matrix (OpenCV)

[0.9999999999999988;

2.1;

-1.4]

Computed Camera Matrix (OpenCV)

[120, -1.4210854715202e-14, 400;

0, 90.00000000000001, 300;

0, 0, 1]

==============================Decomposing Using My Code==============================

Estimated Camera Matrix

[120, -5.776629175002766e-14, 399.9999999999999;

-5.608036291626306e-15, 89.99999999999994, 299.9999999999999;

-1.49192016182457e-17, -1.561251128379126e-16, 1]

Estimated Camera Rotation

[0.8236391035463322, 0.5427868801100542, -0.1643199010764936;

-0.4755282581475767, 0.5031184295835892, -0.7216264418079998;

-0.3090169943749475, 0.6724985119639573, 0.6724985119639575]

Estimated Camera Translation

[1;

2.099999999999999;

-1.4]

Decomposing Projection Using OpenCV and C++ Read More »

Camera Projection Matrix with Eigen

Leave a Comment / C++, Computer Vision, Tutorials / admin

void projectFromCameraCoordinateToCameraPlane( Eigen::Matrix3Xd &pointsInCameraCoordinate, Eigen::Matrix3d &cameraIntrinsicMatrix,Eigen::Matrix2Xd &pointsInCameraPlane)
{
/*
Intrinsic camera Matrix is something like this:

focalLength  ===> unit is mm
mx=  ==>unit is Pixel/mm
U0=  ===> unit is Pixel

my=-(numberOfPixelInHeight)/heightOfSensor ;
U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/heightOfSensor; 
V0=(numberOfPixelInWidth)/2;

Gamma=0

X,Y,Z,W are homogeneous position of point in camera coordinate

┌ ┐	 ┌                         	 ┐┌ ┐
v`	 |f*mx		Gamma		V0	0||X|	
u`	=|0			f*my		U0	0||Y|
w`	 |0			0			 1	0||Z|
└ ┘	 └                         	 ┘└W┘

									 ▲y
									 |	
			----►V					 |	
			|	 ____________________|_____________________
			|	|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
		U	▼	|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|____________► x
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|



To project points from camera coordinate to camera plane:

V`=X*f*mx + V0*Z 
U`=Y*f*my + U0*Z
W`=Z

U=U`/W` ,V=V`/W`


V=f*mx*X/Z + V0 
U=f*my*Y/Z + U0

(U,V) is the index of corresponding point in the image
*/
	
	Eigen::Matrix3Xd pointsInCameraPlaneHomogeneous(pointsInCameraCoordinate.rows(),pointsInCameraCoordinate.cols());	

	pointsInCameraPlaneHomogeneous=cameraIntrinsicMatrix*pointsInCameraCoordinate;
	std::cout<<"pointsInCameraPlaneHomogeneous"<<std::endl;	
 	std::cout<<pointsInCameraPlaneHomogeneous<<std::endl;	
	for(int i=0;i<pointsInCameraPlaneHomogeneous.cols();i++  )
	{
		pointsInCameraPlaneHomogeneous(0,i)=pointsInCameraPlaneHomogeneous(0,i)/pointsInCameraPlaneHomogeneous(2,i);
		pointsInCameraPlaneHomogeneous(1,i)=pointsInCameraPlaneHomogeneous(1,i)/pointsInCameraPlaneHomogeneous(2,i);
	}
	pointsInCameraPlane=pointsInCameraPlaneHomogeneous.block(0,0,pointsInCameraPlaneHomogeneous.rows()-1,pointsInCameraPlaneHomogeneous.cols());
}

void transformPoints(Eigen::Matrix3Xd &points, Eigen::Matrix3d &rotationMatrix, Eigen::Vector3d &translationVector,Eigen::Matrix3Xd &transformedPoints)
{
	transformedPoints=(rotationMatrix*points).colwise() + translationVector ;
}

void projectFromCameraCoordinateToCameraPlane_Example()
{
	
/*
 OpenCV coordinate system
                  ▲
                 /
                /
              Z/
              /
             /
------------------------------► X
            |
            |
            |
            |
          Y |
            ▼
*/	
	
	int numberOfPixelInHeight,numberOfPixelInWidth;
	double heightOfSensor, widthOfSensor;
	double focalLength=1.5;
	double mx, my, U0, V0;
	numberOfPixelInHeight=600;
    numberOfPixelInWidth=600;
	
	heightOfSensor=10;
	widthOfSensor=10;
	
	my=(numberOfPixelInHeight)/heightOfSensor ;
	U0=(numberOfPixelInHeight)/2 ;

	mx=(numberOfPixelInWidth)/widthOfSensor; 
	V0=(numberOfPixelInWidth)/2;
	
	double L = 0.2;
	Eigen::Matrix3Xd controlPointsInWorldCoordinate(3,6);
	Eigen::Matrix3Xd controlPointsInCameraCoordinate(3,6);

	
	Eigen::Matrix3d cameraIntrinsicMatrix;
	cameraIntrinsicMatrix<<focalLength*mx, 0, V0,
						   0,focalLength*my,U0,
						   0,0,1;

	controlPointsInWorldCoordinate.col(0)= Eigen::Vector3d(-L, -L, 0);
	controlPointsInWorldCoordinate.col(1)= Eigen::Vector3d(2 * L, -L, 0.2);
	controlPointsInWorldCoordinate.col(2)= Eigen::Vector3d(L, L, 0.2);
	controlPointsInWorldCoordinate.col(3)= Eigen::Vector3d(-L, L, 0);
	controlPointsInWorldCoordinate.col(4)= Eigen::Vector3d(-2 * L, L, 0);
	controlPointsInWorldCoordinate.col(5)= Eigen::Vector3d(0, 0, 0.5);
	
	
	Eigen::Matrix3d rotationMatrix;
	rotationMatrix<< 1,0,0
					,0,1,0
					,0,0,1;

	Eigen::Vector3d translationVector(3,1);
	translationVector<<-0.1, 0.1, 1.2;
	
	transformPoints(controlPointsInWorldCoordinate, rotationMatrix, translationVector, controlPointsInCameraCoordinate );

	Eigen::Matrix2Xd pointsInCameraPlane(2,controlPointsInCameraCoordinate.cols());
	projectFromCameraCoordinateToCameraPlane(controlPointsInCameraCoordinate,cameraIntrinsicMatrix,pointsInCameraPlane);
    
    
    std::vector<cv::Point3d> cvPointsInCameraCoordinate;
    std::vector<cv::Point2d> cvpointsInCameraPlane;
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, -L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(2 * L, -L, 0.2));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(L, L, 0.2));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-2 * L, L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(0, 0, 0.5));


    cv::Mat cameraMatrix= (cv::Mat_<double>(3,3) <<
    focalLength*mx, 0, V0,
    0,focalLength*my,U0,
    0,0,1);


    cv::Mat rvec= cv::Mat::eye(3,3, CV_64F);
    cv::Mat tvec=(cv::Mat_<double>(3,1)<<-0.1, 0.1, 1.2);
    cv::projectPoints(cvPointsInCameraCoordinate,rvec,tvec,cameraMatrix,cv::noArray(),cvpointsInCameraPlane);
	
    std::cout<<"cameraIntrinsicMatrix" <<std::endl;
    std::cout<<cameraIntrinsicMatrix <<std::endl;

    std::cout<<cameraMatrix <<std::endl;

	
	
    Eigen::MatrixXd image(numberOfPixelInHeight,numberOfPixelInWidth);
    image=MatrixXd::Zero(numberOfPixelInHeight,numberOfPixelInWidth);
    int U,V;
    for(int i=0;i<pointsInCameraPlane.cols();i++)
    {
        U=int(pointsInCameraPlane(0,i));
        V=int(pointsInCameraPlane(1,i));
        std::cout<<U<<","<<V <<std::endl;
        image(U,V)=255;
        std::cout<<cvpointsInCameraPlane.at(i)<<std::endl;

    }
    Mat dst;
    eigen2cv(image, dst);
    std::string fileName=std::string("eigen_camera_projection_result_focal_")+std::to_string(focalLength)+ std::string("_.jpg");
    cv::imwrite(fileName,dst);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

void projectFromCameraCoordinateToCameraPlane( Eigen::Matrix3Xd &pointsInCameraCoordinate, Eigen::Matrix3d &cameraIntrinsicMatrix,Eigen::Matrix2Xd &pointsInCameraPlane)

{

Intrinsic camera Matrix is something like this:

focalLength ===> unit is mm

mx= ==>unit is Pixel/mm

U0= ===> unit is Pixel

my=-(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/heightOfSensor;

V0=(numberOfPixelInWidth)/2;

Gamma=0

X,Y,Z,W are homogeneous position of point in camera coordinate

┌ ┐ ┌ ┐┌ ┐

v` |f*mx Gamma V0 0||X|

u` =|0 f*my U0 0||Y|

w` |0 0 1 0||Z|

└ ┘ └ ┘└W┘

▲y

----►V |

| ____________________|_____________________

| |__|__|__|__|__|__|__|__|__|__|__|__|__|__|

U ▼ |__|__|__|__|__|__|__|__|__|__|__|__|__|__|

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|____________► x

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|

To project points from camera coordinate to camera plane:

V`=X*f*mx + V0*Z

U`=Y*f*my + U0*Z

W`=Z

U=U`/W` ,V=V`/W`

V=f*mx*X/Z + V0

U=f*my*Y/Z + U0

(U,V) is the index of corresponding point in the image

Eigen::Matrix3Xd pointsInCameraPlaneHomogeneous(pointsInCameraCoordinate.rows(),pointsInCameraCoordinate.cols());

pointsInCameraPlaneHomogeneous=cameraIntrinsicMatrix*pointsInCameraCoordinate;

std::cout<<"pointsInCameraPlaneHomogeneous"<<std::endl;

std::cout<<pointsInCameraPlaneHomogeneous<<std::endl;

for(int i=0;i<pointsInCameraPlaneHomogeneous.cols();i++ )

{

pointsInCameraPlaneHomogeneous(0,i)=pointsInCameraPlaneHomogeneous(0,i)/pointsInCameraPlaneHomogeneous(2,i);

pointsInCameraPlaneHomogeneous(1,i)=pointsInCameraPlaneHomogeneous(1,i)/pointsInCameraPlaneHomogeneous(2,i);

}

pointsInCameraPlane=pointsInCameraPlaneHomogeneous.block(0,0,pointsInCameraPlaneHomogeneous.rows()-1,pointsInCameraPlaneHomogeneous.cols());

}

void transformPoints(Eigen::Matrix3Xd &points, Eigen::Matrix3d &rotationMatrix, Eigen::Vector3d &translationVector,Eigen::Matrix3Xd &transformedPoints)

{

transformedPoints=(rotationMatrix*points).colwise() + translationVector ;

}

void projectFromCameraCoordinateToCameraPlane_Example()

{

OpenCV coordinate system

▲

------------------------------► X

Y |

▼

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=1.5;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=600;

heightOfSensor=10;

widthOfSensor=10;

my=(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/widthOfSensor;

V0=(numberOfPixelInWidth)/2;

double L = 0.2;

Eigen::Matrix3Xd controlPointsInWorldCoordinate(3,6);

Eigen::Matrix3Xd controlPointsInCameraCoordinate(3,6);

Eigen::Matrix3d cameraIntrinsicMatrix;

cameraIntrinsicMatrix<<focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1;

controlPointsInWorldCoordinate.col(0)= Eigen::Vector3d(-L, -L, 0);

controlPointsInWorldCoordinate.col(1)= Eigen::Vector3d(2 * L, -L, 0.2);

controlPointsInWorldCoordinate.col(2)= Eigen::Vector3d(L, L, 0.2);

controlPointsInWorldCoordinate.col(3)= Eigen::Vector3d(-L, L, 0);

controlPointsInWorldCoordinate.col(4)= Eigen::Vector3d(-2 * L, L, 0);

controlPointsInWorldCoordinate.col(5)= Eigen::Vector3d(0, 0, 0.5);

Eigen::Matrix3d rotationMatrix;

rotationMatrix<< 1,0,0

,0,1,0

,0,0,1;

Eigen::Vector3d translationVector(3,1);

translationVector<<-0.1, 0.1, 1.2;

transformPoints(controlPointsInWorldCoordinate, rotationMatrix, translationVector, controlPointsInCameraCoordinate );

Eigen::Matrix2Xd pointsInCameraPlane(2,controlPointsInCameraCoordinate.cols());

projectFromCameraCoordinateToCameraPlane(controlPointsInCameraCoordinate,cameraIntrinsicMatrix,pointsInCameraPlane);

std::vector<cv::Point3d> cvPointsInCameraCoordinate;

std::vector<cv::Point2d> cvpointsInCameraPlane;

cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, -L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(2 * L, -L, 0.2));

cvPointsInCameraCoordinate.push_back(cv::Point3d(L, L, 0.2));

cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(-2 * L, L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(0, 0, 0.5));

cv::Mat cameraMatrix= (cv::Mat_<double>(3,3) <<

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

cv::Mat rvec= cv::Mat::eye(3,3, CV_64F);

cv::Mat tvec=(cv::Mat_<double>(3,1)<<-0.1, 0.1, 1.2);

cv::projectPoints(cvPointsInCameraCoordinate,rvec,tvec,cameraMatrix,cv::noArray(),cvpointsInCameraPlane);

std::cout<<"cameraIntrinsicMatrix" <<std::endl;

std::cout<<cameraIntrinsicMatrix <<std::endl;

std::cout<<cameraMatrix <<std::endl;

Eigen::MatrixXd image(numberOfPixelInHeight,numberOfPixelInWidth);

image=MatrixXd::Zero(numberOfPixelInHeight,numberOfPixelInWidth);

int U,V;

for(int i=0;i<pointsInCameraPlane.cols();i++)

{

U=int(pointsInCameraPlane(0,i));

V=int(pointsInCameraPlane(1,i));

std::cout<<U<<","<<V <<std::endl;

image(U,V)=255;

std::cout<<cvpointsInCameraPlane.at(i)<<std::endl;

}

Mat dst;

eigen2cv(image, dst);

std::string fileName=std::string("eigen_camera_projection_result_focal_")+std::to_string(focalLength)+ std::string("_.jpg");

cv::imwrite(fileName,dst);

}

Camera Projection Matrix with Eigen Read More »

Create OpenCV Matrix (cv::Mat, cv::Mat_ ) From Various Data Types

Leave a Comment / C++, Tutorials / admin

1) cv:: Mat_ You can create a matrix with cv:: Mat_

cv::Mat cameraMatrix(3, 3, cv::DataType::type);
cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<focalLength*mx, 0, V0,0,focalLength*my,U0,0,0,1);

1 2	cv::Mat cameraMatrix(3, 3, cv::DataType::type); cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<focalLengthmx, 0, V0,0,focalLengthmy,U0,0,0,1);

2) cv::Mat or like this with cv::Mat

/*
              C1   C2    C3    C4
    CV_8U     0    8     16    24
    CV_8S     1    9     17    25
    CV_16U    2    10    18    26
    CV_16S    3    11    19    27
    CV_32S    4    12    20    28
    CV_32F    5    13    21    29
    CV_64F    6    14    22    30
*/

cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);

C1 C2 C3 C4

CV_8U 0 8 16 24

CV_8S 1 9 17 25

CV_16U 2 10 18 26

CV_16S 3 11 19 27

CV_32S 4 12 20 28

CV_32F 5 13 21 29

CV_64F 6 14 22 30

cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);

if you forgot the list of data types you use

cv::Mat A(number_of_rows, number_of_columns, cv::DataType<float>::type);

1	cv::Mat A(number_of_rows, number_of_columns, cv::DataType<float>::type);

more data types here

Create OpenCV Matrix (cv::Mat, cv::Mat_ ) From Various Data Types Read More »

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV

Leave a Comment / C++, Computer Vision, Tutorials / admin

Following my other post, you can extract the equation for epipolar lines. Basically choosing one point in one image and using fundamental matrix, we will get a line in the other image:

    /*
    FM_7POINT   7-point algorithm
    FM_8POINT   8-point algorithm
    FM_LMEDS    least-median algorithm. 7-point algorithm is used.
    FM_RANSAC   ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used
    */	

    cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);
    std::vector<cv::Vec3d> leftLines, rightLines;
    cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);
    cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);
	
    cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);
    cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);
	
    cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);
    cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

    cv::Mat imagePointLeftCameraMatrix=cv::Mat_<double>(3,1);

    for(std::size_t i=0;i<rightLines.size();i=i+1)
    {
        cv::Vec3d l=rightLines.at(i);
	double a=l.val[0];
        double b=l.val[1];
        double c=l.val[2];
        std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;
        std::cout<< a <<", "<<b <<", "<<c <<std::endl;
        std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;
        
        imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;
        imagePointLeftCameraMatrix.at<double>(1,0)=imagePointsLeftCamera[i].y;
        imagePointLeftCameraMatrix.at<double>(2,0)=1;
        cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;
        
        std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

        /////////////////////////////////drawing the line on the image/////////////////////////////////
        /*ax+by+c=0*/
        double x0,y0,x1,y1;
        x0=0;
        y0=(-c-a*x0)/b;
	x1=rightImageRGB.cols;
        y1=(-c-a*x1)/b;

	std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;
	cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);
    }

    cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

FM_7POINT 7-point algorithm

FM_8POINT 8-point algorithm

FM_LMEDS least-median algorithm. 7-point algorithm is used.

FM_RANSAC ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used

cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);

std::vector<cv::Vec3d> leftLines, rightLines;

cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);

cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);

cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);

cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);

cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);

cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

cv::Mat imagePointLeftCameraMatrix=cv::Mat_<double>(3,1);

for(std::size_t i=0;i<rightLines.size();i=i+1)

{

cv::Vec3d l=rightLines.at(i);

double a=l.val[0];

double b=l.val[1];

double c=l.val[2];

std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;

std::cout<< a <<", "<<b <<", "<<c <<std::endl;

std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;

imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;

imagePointLeftCameraMatrix.at<double>(1,0)=imagePointsLeftCamera[i].y;

imagePointLeftCameraMatrix.at<double>(2,0)=1;

cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;

std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

/////////////////////////////////drawing the line on the image/////////////////////////////////

/*ax+by+c=0*/

double x0,y0,x1,y1;

x0=0;

y0=(-c-a*x0)/b;

x1=rightImageRGB.cols;

y1=(-c-a*x1)/b;

std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;

cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);

}

cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV Read More »

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.